vecN<vecN<vecN<OutputType, 2>, 4>, 2> split_cubicT(c_array<const vecN<InputType, 2> > pts) { FASTUIDRAWassert(pts.size() == 4); vecN<vecN<vecN<OutputType, 2>, 4>, 2> return_value; vecN<IntermediateType, 2> p0(pts[0]), p1(pts[1]), p2(pts[2]), p3(pts[3]); vecN<IntermediateType, 2> p01, p23, pA, pB, pC; const IntermediateType two(2), three(3), four(4), eight(8); p01 = (p0 + p1) / two; p23 = (p2 + p3) / two; pA = (p0 + two * p1 + p2) / four; pB = (p1 + two * p2 + p3) / four; pC = (p0 + three * p1 + three * p2 + p3) / eight; vecN<OutputType, 2> q0(pts[0]), q01(p01), qA(pA), qC(pC); vecN<OutputType, 2> qB(pB), q23(p23), q3(pts[3]); return_value[0] = vecN<vecN<OutputType, 2>, 4>(q0, q01, qA, qC); return_value[1] = vecN<vecN<OutputType, 2>, 4>(qC, qB, q23, q3); return return_value; }
double C2M::getHit(Vec3 p3, Vec3 n3){ Vec3 p(p3); Vec3 n((n3 - Eye::get()->N*(n3*Eye::get()->N) ).normalize()); double tmin = 999999999999; bool hit = false; Vec3 hitp; for(list<ArrCurve*>::iterator it =_curves.begin(); it!=_curves.end(); it++){ ArrCurve* ac = (*it); Vec3 p0(ac->getP(0)); for(int i=1; i<ac->size();i++){ Vec3 p1(ac->getP(i)); Vec3 n1 = (p1 - p0); //double d = (n1%n).normalize()*((p0-p)%n1).normalize(); //cout<<"dot:="<<d<<"........................"<<endl; double len1 = n1.norm(); double t = getIntersectionDist(p, n, p1, n1.normalize() ); if (t>0 && t<tmin && ( p + t*n - p1 ).norm()<len1 ){ tmin = t; hit = true; } p0.set(p1); } } if (hit) return tmin; return -1; }
void MapgenV6::addDirtGravelBlobs() { if (getBiome(v2s16(node_min.X, node_min.Z)) != BT_NORMAL) return; PseudoRandom pr(blockseed + 983); for (int i = 0; i < volume_nodes/10/10/10; i++) { bool only_fill_cave = (myrand_range(0,1) != 0); v3s16 size( pr.range(1, 8), pr.range(1, 8), pr.range(1, 8) ); v3s16 p0( pr.range(node_min.X, node_max.X) - size.X / 2, pr.range(node_min.Y, node_max.Y) - size.Y / 2, pr.range(node_min.Z, node_max.Z) - size.Z / 2 ); MapNode n1((p0.Y > -32 && !pr.range(0, 1)) ? c_dirt : c_gravel); for (int z1 = 0; z1 < size.Z; z1++) for (int y1 = 0; y1 < size.Y; y1++) for (int x1 = 0; x1 < size.X; x1++) { v3s16 p = p0 + v3s16(x1, y1, z1); u32 i = vm->m_area.index(p); if (!vm->m_area.contains(i)) continue; // Cancel if not stone and not cave air if (vm->m_data[i].getContent() != c_stone && !(vm->m_flags[i] & VMANIP_FLAG_CAVE)) continue; if (only_fill_cave && !(vm->m_flags[i] & VMANIP_FLAG_CAVE)) continue; vm->m_data[i] = n1; } } }
dgFloat32 dgFastRayTest::PolygonIntersect (const dgVector& faceNormal, dgFloat32 maxT, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) const { dgAssert (m_p0.m_w == dgFloat32 (0.0f)); dgAssert (m_p1.m_w == dgFloat32 (0.0f)); if (faceNormal.DotProduct(m_unitDir).GetScalar() < dgFloat32 (0.0f)) { dgInt32 stride = dgInt32(strideInBytes / sizeof (dgFloat32)); dgBigVector v0(dgVector(&polygon[indexArray[indexCount - 1] * stride]) & dgVector::m_triplexMask); dgBigVector p0(m_p0); dgBigVector p0v0(v0 - p0); dgBigVector diff(m_diff); dgBigVector normal(faceNormal); dgFloat64 tOut = normal.DotProduct(p0v0).GetScalar() / normal.DotProduct(diff).GetScalar(); if ((tOut >= dgFloat64(0.0f)) && (tOut <= maxT)) { dgBigVector p (p0 + diff.Scale (tOut)); dgBigVector unitDir(m_unitDir); for (dgInt32 i = 0; i < indexCount; i++) { dgInt32 i2 = indexArray[i] * stride; dgBigVector v1(dgVector(&polygon[i2]) & dgVector::m_triplexMask); dgBigVector edge0(p - v0); dgBigVector edge1(v1 - v0); dgFloat64 area = unitDir.DotProduct (edge0.CrossProduct(edge1)).GetScalar(); if (area < dgFloat32 (0.0f)) { return 1.2f; } v0 = v1; } return dgFloat32(tOut); } } return dgFloat32 (1.2f); }
void object::test<1> () { LineIntersector li; Coordinate pt(10, 10); HotPixel hp(pt, 1, li); ensure_equals(hp.getCoordinate(), pt); const Envelope& env = hp.getSafeEnvelope(); ensure_equals(env.toString(), "Env[9.25:10.75,9.25:10.75]"); Coordinate p0(0, 10); Coordinate p1(20, 10); ensure("hp.intersects 0 10, 20 10", hp.intersects(p0, p1)); p1.y = 11; // intersection point within 0.75 distance ensure("hp.intersects(0 10, 20 11)", hp.intersects(p0, p1)); p1.y = 20; ensure_not("!hp.intersects(0 10, 20 20)", hp.intersects(p0, p1)); }
static void RenderBodyContactsAndTangentDiretions (NewtonBody* const body, float length) { for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(body); joint; joint = NewtonBodyGetNextContactJoint(body, joint)) { if (NewtonJointIsActive (joint)) { for (void* contact = NewtonContactJointGetFirstContact (joint); contact; contact = NewtonContactJointGetNextContact (joint, contact)) { dVector point; dVector normal; NewtonMaterial* const material = NewtonContactGetMaterial (contact); NewtonMaterialGetContactPositionAndNormal (material, body, &point.m_x, &normal.m_x); dVector tangnetDir0; dVector tangnetDir1; NewtonMaterialGetContactTangentDirections(material, body, &tangnetDir0[0], &tangnetDir1[0]); // if we are display debug info we need to block other threads from writing the data at the same time dVector p1 (point + normal.Scale (length)); //dVector p0 (point - normal.Scale (length)); dVector p0 (point); glVertex3f (p0.m_x, p0.m_y, p0.m_z); glVertex3f (p1.m_x, p1.m_y, p1.m_z); } } } }
GUIFormSpecMenu::ItemSpec GUIFormSpecMenu::getItemAtPos(v2s32 p) const { core::rect<s32> imgrect(0,0,imgsize.X,imgsize.Y); for(u32 i=0; i<m_inventorylists.size(); i++) { const ListDrawSpec &s = m_inventorylists[i]; for(s32 i=0; i<s.geom.X*s.geom.Y; i++) { s32 item_i = i + s.start_item_i; s32 x = (i%s.geom.X) * spacing.X; s32 y = (i/s.geom.X) * spacing.Y; v2s32 p0(x,y); core::rect<s32> rect = imgrect + s.pos + p0; if(rect.isPointInside(p)) { return ItemSpec(s.inventoryloc, s.listname, item_i); } } } return ItemSpec(InventoryLocation(), "", -1); }
void setup(MeshType & mesh, viennagrid::quadrilateral_tag) { typedef typename viennagrid::result_of::point<MeshType>::type PointType; typedef typename viennagrid::result_of::vertex_handle<MeshType>::type VertexHandleType; PointType p0(0.0, 0.0); PointType p1(1.0, 0.0); PointType p2(2.0, 0.0); PointType p3(2.0, 1.0); PointType p4(1.0, 1.0); PointType p5(0.0, 1.0); std::cout << "Adding vertices to segment:" << std::endl; VertexHandleType vh0 = viennagrid::make_vertex( mesh, p0 ); VertexHandleType vh1 = viennagrid::make_vertex( mesh, p1 ); VertexHandleType vh2 = viennagrid::make_vertex( mesh, p2 ); VertexHandleType vh3 = viennagrid::make_vertex( mesh, p3 ); VertexHandleType vh4 = viennagrid::make_vertex( mesh, p4 ); VertexHandleType vh5 = viennagrid::make_vertex( mesh, p5 ); viennagrid::make_quadrilateral( mesh, vh0, vh1, vh5, vh4 ); viennagrid::make_quadrilateral( mesh, vh1, vh2, vh4, vh3 ); }
void RemoveMultiple ( const std::list<ID>& listToTreat, std::list< std::pair<ID, ID> >& finalList ) { ID counter = 0; std::list<ID> temporaryList ( listToTreat ); //! Sort the list temporaryList.sort(); //! initialize the new list std::pair <ID, ID> p0 ( temporaryList.front() , counter ); finalList.push_back ( p0 ); //! We remove the multiple occurences : for ( std::list<ID>::iterator it = temporaryList.begin() ; it != temporaryList.end() ; ++ it ) { if ( ( *it ) != finalList.back().first ) { counter ++ ; //! Add to the list the new value std::pair <ID, ID> p ( ( *it ) , counter ); finalList.push_back ( p ); } } }
int main(int argc,char **argv) { if(argc!=2){ fprintf(stderr,"usage: %s scanner.so\n",argv[0]); fprintf(stderr,"type 'make plugins' to make available plugins\n"); exit(1); } /* Strip extension and path */ std::string fname = argv[1]; scanner_t *fn=0; std::string name = fname; size_t dot = name.rfind('.'); if(dot==std::string::npos){ fprintf(stderr,"%s: cannot strip extension\n",name.c_str()); exit(1); } name = name.substr(0,dot); /* Strip dir */ size_t slash = name.rfind('.'); if(slash!=std::string::npos){ name = name.substr(slash+1); } #ifdef HAVE_DLOPEN if(fname.find('.')==std::string::npos){ fname = "./" + fname; // fedora requires a complete path name } #ifdef HAVE_DLOPEN_PREFLIGHT if(!dlopen_preflight(fname.c_str())){ fprintf(stderr,"dlopen_preflight - cannot open %s: %s",fname.c_str(),dlerror()); exit(1); } #endif void *lib=dlopen(fname.c_str(), RTLD_LAZY); if(lib==0){ fprintf(stderr,"fname=%s\n",fname.c_str()); fprintf(stderr,"dlopen: %s\n",dlerror()); exit(1); } fn=(scanner_t *)dlsym(lib, name.c_str()); if(fn==0){ fprintf(stderr,"dlsym: %s\n",dlerror()); exit(1); } #endif #ifdef HAVE_LOADLIBRARY /* Use Win32 LoadLibrary function */ /* See http://msdn.microsoft.com/en-us/library/ms686944(v=vs.85).aspx */ HINSTANCE hinstLib = LoadLibrary(TEXT(fname.c_str())); if(hinstLib==0){ fprintf(stderr,"LoadLibrary(%s) failed",fname.c_str()); exit(1); } MYPROC fn = (MYPROC)GetProcAddress(hinstLib,name.c_str()); if(fn==0){ fprintf(stderr,"GetProcAddress(%s) failed",name.c_str()); exit(1); } #endif feature_recorder_set fs(0,my_hasher,feature_recorder_set::NO_INPUT,feature_recorder_set::NO_OUTDIR); uint8_t buf[100]; pos0_t p0(""); sbuf_t sbuf(p0,buf,sizeof(buf),sizeof(buf),false); scanner_params sp(scanner_params::PHASE_STARTUP,sbuf,fs); recursion_control_block rcb(0,"STAND"); scanner_info si; sp.info = &si; (*fn)(sp,rcb); std::cout << "Loaded scanner '" << si.name << "' by " << si.author << "\n"; #ifdef HAVE_DLOPEN dlclose(lib); #endif return 0; }
void Camera::DrawSpike(double rad, const vector3d &viewCoords, const matrix4x4d &viewTransform) { glPushMatrix(); float znear, zfar; Render::GetNearFarClipPlane(znear, zfar); double newdist = znear + 0.5f * (zfar - znear); double scale = newdist / viewCoords.Length(); glTranslatef(float(scale*viewCoords.x), float(scale*viewCoords.y), float(scale*viewCoords.z)); Render::State::UseProgram(0); // face the camera dammit vector3d zaxis = viewCoords.Normalized(); vector3d xaxis = vector3d(0,1,0).Cross(zaxis).Normalized(); vector3d yaxis = zaxis.Cross(xaxis); matrix4x4d rot = matrix4x4d::MakeInvRotMatrix(xaxis, yaxis, zaxis); glMultMatrixd(&rot[0]); glDisable(GL_LIGHTING); glDisable(GL_DEPTH_TEST); glEnable(GL_BLEND); // XXX WRONG. need to pick light from appropriate turd. GLfloat col[4]; glGetLightfv(GL_LIGHT0, GL_DIFFUSE, col); glColor4f(col[0], col[1], col[2], 1); glBegin(GL_TRIANGLE_FAN); glVertex3f(0,0,0); glColor4f(col[0], col[1], col[2], 0); const float spikerad = float(scale*rad); // bezier with (0,0,0) control points { vector3f p0(0,spikerad,0), p1(spikerad,0,0); float t=0.1f; for (int i=1; i<10; i++, t+= 0.1f) { vector3f p = (1-t)*(1-t)*p0 + t*t*p1; glVertex3fv(&p[0]); } } { vector3f p0(spikerad,0,0), p1(0,-spikerad,0); float t=0.1f; for (int i=1; i<10; i++, t+= 0.1f) { vector3f p = (1-t)*(1-t)*p0 + t*t*p1; glVertex3fv(&p[0]); } } { vector3f p0(0,-spikerad,0), p1(-spikerad,0,0); float t=0.1f; for (int i=1; i<10; i++, t+= 0.1f) { vector3f p = (1-t)*(1-t)*p0 + t*t*p1; glVertex3fv(&p[0]); } } { vector3f p0(-spikerad,0,0), p1(0,spikerad,0); float t=0.1f; for (int i=1; i<10; i++, t+= 0.1f) { vector3f p = (1-t)*(1-t)*p0 + t*t*p1; glVertex3fv(&p[0]); } } glEnd(); glDisable(GL_BLEND); glEnable(GL_LIGHTING); glEnable(GL_DEPTH_TEST); glPopMatrix(); }
dgUnsigned32 dgSlidingConstraint::JacobianDerivative (dgContraintDescritor& params) { dgMatrix matrix0; dgMatrix matrix1; //dgVector angle (CalculateGlobalMatrixAndAngle (matrix0, matrix1)); CalculateGlobalMatrixAndAngle (matrix0, matrix1); m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front; matrix1.m_posit += matrix1.m_front.Scale3 (m_posit); dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_front % matrix0.m_front)) < dgFloat32 (1.0e-5f)); dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_up % matrix0.m_up)) < dgFloat32 (1.0e-5f)); dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_right % matrix0.m_right)) < dgFloat32 (1.0e-5f)); const dgVector& dir1 = matrix0.m_up; const dgVector& dir2 = matrix0.m_right; dgVector p0 (matrix0.m_posit); dgVector p1 (matrix1.m_posit + matrix1.m_front.Scale3 ((p0 - matrix1.m_posit) % matrix1.m_front)); dgVector q0 (p0 + matrix0.m_front.Scale3(MIN_JOINT_PIN_LENGTH)); dgVector q1 (p1 + matrix1.m_front.Scale3(MIN_JOINT_PIN_LENGTH)); dgVector r0 (p0 + matrix0.m_up.Scale3(MIN_JOINT_PIN_LENGTH)); dgVector r1 (p1 + matrix1.m_up.Scale3(MIN_JOINT_PIN_LENGTH)); dgPointParam pointDataP; dgPointParam pointDataQ; dgPointParam pointDataR; InitPointParam (pointDataP, m_stiffness, p0, p1); InitPointParam (pointDataQ, m_stiffness, q0, q1); InitPointParam (pointDataR, m_stiffness, r0, r1); CalculatePointDerivative (0, params, dir1, pointDataP, &m_jointForce[0]); CalculatePointDerivative (1, params, dir2, pointDataP, &m_jointForce[1]); CalculatePointDerivative (2, params, dir1, pointDataQ, &m_jointForce[2]); CalculatePointDerivative (3, params, dir2, pointDataQ, &m_jointForce[3]); CalculatePointDerivative (4, params, dir2, pointDataR, &m_jointForce[4]); dgInt32 ret = 5; if (m_jointAccelFnt) { dgJointCallbackParam axisParam; axisParam.m_accel = dgFloat32 (0.0f); axisParam.m_timestep = params.m_timestep; axisParam.m_minFriction = DG_MIN_BOUND; axisParam.m_maxFriction = DG_MAX_BOUND; if (m_jointAccelFnt (*this, &axisParam)) { if ((axisParam.m_minFriction > DG_MIN_BOUND) || (axisParam.m_maxFriction < DG_MAX_BOUND)) { params.m_forceBounds[5].m_low = axisParam.m_minFriction; params.m_forceBounds[5].m_upper = axisParam.m_maxFriction; params.m_forceBounds[5].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT; } CalculatePointDerivative (5, params, matrix0.m_front, pointDataP, &m_jointForce[5]); //params.m_jointAccel[5] = axisParam.m_accel; SetMotorAcceleration (5, axisParam.m_accel, params); ret = 6; } } return dgUnsigned32 (ret); }
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale) { dgAssert(proxy.m_referenceCollision->IsType(dgCollision::dgCollisionConvexShape_RTTI)); dgAssert(proxy.m_floatingCollision->IsType(dgCollision::dgCollisionConvexPolygon_RTTI)); const dgCollisionInstance* const polygonInstance = proxy.m_floatingCollision; dgAssert(this == polygonInstance->GetChildShape()); dgAssert(m_count); dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0]))); dgInt32 count = 0; m_normal = m_normal.CompProduct4(polyInstanceInvScale); dgAssert(m_normal.m_w == dgFloat32(0.0f)); m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt()); dgVector savedFaceNormal(m_normal); dgVector savedPosit (proxy.m_matrix.m_posit); proxy.m_matrix.m_posit = dgVector::m_wOne; dgVector hullOrigin(proxy.m_matrix.UnrotateVector (savedPosit)); for (dgInt32 i = 0; i < m_count; i++) { m_localPoly[i] = hullOrigin + polyInstanceScale.CompProduct4(dgVector(&m_vertex[m_vertexIndex[i] * m_stride])); dgAssert(m_localPoly[i].m_w == dgFloat32(0.0f)); } dgContact* const contactJoint = proxy.m_contactJoint; const dgCollisionInstance* const hull = proxy.m_referenceCollision; dgVector normalInHull(proxy.m_matrix.RotateVector(m_normal)); dgVector pointInHull(hull->SupportVertex(normalInHull.Scale4(dgFloat32(-1.0f)), NULL)); dgVector p0(proxy.m_matrix.UntransformVector(pointInHull)); dgVector p1(proxy.m_matrix.UntransformVector(hull->SupportVertex(normalInHull, NULL))); dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness; if (penetration < dgFloat32(0.0f)) { contactJoint->m_closestDistance = -penetration; proxy.m_matrix.m_posit = savedPosit; return 0; } contactJoint->m_closestDistance = dgFloat32(0.0f); dgFloat32 distance = (m_localPoly[0] - p1) % m_normal; if (distance >= dgFloat32(0.0f)) { proxy.m_matrix.m_posit = savedPosit; return 0; } dgVector boxSize (hull->GetBoxSize() & dgVector::m_triplexMask); dgVector boxOrigin ((hull->GetBoxOrigin() & dgVector::m_triplexMask) + dgVector::m_wOne); bool inside = true; dgInt32 i0 = m_count - 1; for (dgInt32 i = 0; i < m_count; i++) { dgVector e(m_localPoly[i] - m_localPoly[i0]); dgVector n(m_normal * e); //dgPlane plane(n, -(m_localPoly[i0] % n)); dgPlane plane(n, - m_localPoly[i0].DotProduct4 (n).GetScalar()); plane = proxy.m_matrix.TransformPlane(plane); //dgFloat32 supportDist = dgAbsf(plane.m_x) * boxSize.m_x + dgAbsf(plane.m_y) * boxSize.m_y + dgAbsf(plane.m_z) * boxSize.m_z; //dgFloat32 centerDist = plane.Evalue(boxOrigin); dgFloat32 supportDist = boxSize.DotProduct4 (plane.Abs()).GetScalar(); dgFloat32 centerDist = plane.DotProduct4 (boxOrigin).GetScalar(); if ((centerDist + supportDist) < dgFloat32(0.0f)) { proxy.m_matrix.m_posit = savedPosit; return 0; } if ((centerDist - supportDist) < dgFloat32(0.0f)) { inside = false; break; } i0 = i; } const dgInt32 hullId = hull->GetUserDataID(); if (inside & !proxy.m_intersectionTestOnly) { dgAssert(penetration >= dgFloat32(0.0f)); dgVector pointsContacts[64]; dgAssert(penetration >= 0.0f); dgVector point(pointInHull + normalInHull.Scale4(penetration)); count = hull->CalculatePlaneIntersection(normalInHull.Scale4(dgFloat32(-1.0f)), point, pointsContacts, 1.0f); dgVector step(normalInHull.Scale4((proxy.m_skinThickness - penetration) * dgFloat32(0.5f))); const dgMatrix& worldMatrix = hull->m_globalMatrix; dgContactPoint* const contactsOut = proxy.m_contacts; dgAssert(contactsOut); dgVector globalNormal(worldMatrix.RotateVector(normalInHull)); for (dgInt32 i = 0; i < count; i++) { contactsOut[i].m_point = worldMatrix.TransformVector(pointsContacts[i] + step); contactsOut[i].m_normal = globalNormal; contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; contactsOut[i].m_penetration = penetration; } } else { dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize(); if (m_faceClipSize > convexSphapeUmbra) { BeamClipping(dgVector(dgFloat32(0.0f)), convexSphapeUmbra); m_faceClipSize = hull->m_childShape->GetBoxMaxRadius(); } dgCollisionConvex* const convexShape = (dgCollisionConvex*)hull->m_childShape; count = convexShape->CalculateConvexToConvexContact(proxy); dgAssert(proxy.m_intersectionTestOnly || (count >= 0)); if (count >= 1) { dgContactPoint* const contactsOut = proxy.m_contacts; if (m_closestFeatureType == 3) { for (dgInt32 i = 0; i < count; i++) { //contactsOut[i].m_userId = m_faceId; contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; } } else { dgVector normal(polygonInstance->m_globalMatrix.UnrotateVector(contactsOut[0].m_normal)); if (normal.DotProduct4(savedFaceNormal).GetScalar() < dgFloat32(0.9995f)) { dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex]; dgVector n(&m_vertex[index * m_stride]); if ((savedFaceNormal.DotProduct4(n).GetScalar() > dgFloat32(0.9995f))) { normal = n; } else { dgVector dir0(n * savedFaceNormal); dgVector dir1(n * normal); dgFloat32 projection = dir0.DotProduct4(dir1).GetScalar(); if (projection <= dgFloat32(0.0f)) { normal = n; } } normal = polygonInstance->m_globalMatrix.RotateVector(normal); for (dgInt32 i = 0; i < count; i++) { contactsOut[i].m_normal = normal; //contactsOut[i].m_userId = m_faceId; contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; } } else { for (dgInt32 i = 0; i < count; i++) { //contactsOut[i].m_userId = m_faceId; contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; } } } } } proxy.m_matrix.m_posit = savedPosit; return count; }
dgInt32 dgCollisionConvexPolygon::CalculatePlaneIntersection (const dgVector& normalIn, const dgVector& origin, dgVector* const contactsOut, dgFloat32 normalSign) const { dgVector normal(normalIn); dgInt32 count = 0; dgFloat32 maxDist = dgFloat32 (1.0f); dgFloat32 projectFactor = m_normal % normal; if (projectFactor < dgFloat32 (0.0f)) { projectFactor *= dgFloat32 (-1.0f); normal = normal.Scale3 (dgFloat32 (-1.0f)); } if (projectFactor > dgFloat32 (0.9999f)) { for (dgInt32 i = 0; i < m_count; i ++) { contactsOut[count] = m_localPoly[i]; count ++; } #ifdef _DEBUG dgInt32 j = count - 1; for (dgInt32 i = 0; i < count; i ++) { dgVector error (contactsOut[i] - contactsOut[j]); dgAssert ((error % error) > dgFloat32 (1.0e-20f)); j = i; } #endif } else if (projectFactor > dgFloat32 (0.1736f)) { maxDist = dgFloat32 (0.0f); dgPlane plane (normal, - (normal % origin)); dgVector p0 (m_localPoly[m_count - 1]); dgFloat32 side0 = plane.Evalue (p0); for (dgInt32 i = 0; i < m_count; i ++) { dgVector p1 (m_localPoly[i]); dgFloat32 side1 = plane.Evalue (p1); if (side0 > dgFloat32 (0.0f)) { maxDist = dgMax (maxDist, side0); contactsOut[count] = p0 - plane.Scale3 (side0); count ++; if (count > 1) { dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]); dgFloat32 error = edgeSegment % edgeSegment; if (error < dgFloat32 (1.0e-8f)) { count --; } } if (side1 <= dgFloat32 (0.0f)) { dgVector dp (p1 - p0); dgFloat32 t = plane % dp; dgAssert (dgAbsf (t) >= dgFloat32 (0.0f)); if (dgAbsf (t) < dgFloat32 (1.0e-8f)) { t = dgSign(t) * dgFloat32 (1.0e-8f); } contactsOut[count] = p0 - dp.Scale3 (side0 / t); count ++; if (count > 1) { dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]); dgFloat32 error = edgeSegment % edgeSegment; if (error < dgFloat32 (1.0e-8f)) { count --; } } } } else if (side1 > dgFloat32 (0.0f)) { dgVector dp (p1 - p0); dgFloat32 t = plane % dp; dgAssert (dgAbsf (t) >= dgFloat32 (0.0f)); if (dgAbsf (t) < dgFloat32 (1.0e-8f)) { t = dgSign(t) * dgFloat32 (1.0e-8f); } contactsOut[count] = p0 - dp.Scale3 (side0 / t); count ++; if (count > 1) { dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]); dgFloat32 error = edgeSegment % edgeSegment; if (error < dgFloat32 (1.0e-8f)) { count --; } } } side0 = side1; p0 = p1; } } else { maxDist = dgFloat32 (1.0e10f); dgPlane plane (normal, - (normal % origin)); dgVector p0 (m_localPoly[m_count - 1]); dgFloat32 side0 = plane.Evalue (p0); for (dgInt32 i = 0; i < m_count; i ++) { dgVector p1 (m_localPoly[i]); dgFloat32 side1 = plane.Evalue (p1); if ((side0 * side1) < dgFloat32 (0.0f)) { dgVector dp (p1 - p0); dgFloat32 t = plane % dp; dgAssert (dgAbsf (t) >= dgFloat32 (0.0f)); if (dgAbsf (t) < dgFloat32 (1.0e-8f)) { t = dgSign(t) * dgFloat32 (1.0e-8f); } contactsOut[count] = p0 - dp.Scale3 (side0 / t); count ++; if (count > 1) { dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]); dgFloat32 error = edgeSegment % edgeSegment; if (error < dgFloat32 (1.0e-8f)) { count --; } } } side0 = side1; p0 = p1; } } if (count > 1) { if (maxDist < dgFloat32 (1.0e-3f)) { dgVector maxPoint (contactsOut[0]); dgVector minPoint (contactsOut[0]); dgVector lineDir (m_normal * normal); dgFloat32 proj = contactsOut[0] % lineDir; dgFloat32 maxProjection = proj; dgFloat32 minProjection = proj; for (dgInt32 i = 1; i < count; i ++) { proj = contactsOut[i] % lineDir; if (proj > maxProjection) { maxProjection = proj; maxPoint = contactsOut[i]; } if (proj < minProjection) { minProjection = proj; minPoint = contactsOut[i]; } } contactsOut[0] = maxPoint; contactsOut[1] = minPoint; count = 2; } dgVector error (contactsOut[count - 1] - contactsOut[0]); if ((error % error) < dgFloat32 (1.0e-8f)) { count --; } } #ifdef _DEBUG if (count > 1) { dgInt32 j = count - 1; for (dgInt32 i = 0; i < count; i ++) { dgVector error (contactsOut[i] - contactsOut[j]); dgAssert ((error % error) > dgFloat32 (1.0e-20f)); j = i; } if (count >= 3) { dgVector n (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); dgVector e0 (contactsOut[1] - contactsOut[0]); for (dgInt32 i = 2; i < count; i ++) { dgVector e1 (contactsOut[i] - contactsOut[0]); n += e0 * e1; e0 = e1; } n = n.Scale3 (dgRsqrt(n % n)); dgFloat32 val = n % normal; dgAssert (val > dgFloat32 (0.9f)); } } #endif return count; }
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy) { dgInt32 count = 0; dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI)); dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI)); dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity()); const dgCollisionInstance* const polygonInstance = proxy.m_instance1; dgAssert(this == polygonInstance->GetChildShape()); dgAssert(m_count); dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0]))); const dgMatrix& hullMatrix = proxy.m_instance0->m_globalMatrix; dgContact* const contactJoint = proxy.m_contactJoint; const dgCollisionInstance* const hull = proxy.m_instance0; dgVector normalInHull(hullMatrix.UnrotateVector(m_normal)); dgVector pointInHull(hull->SupportVertex(normalInHull.Scale4(dgFloat32(-1.0f)), NULL)); dgVector p0(hullMatrix.TransformVector(pointInHull)); dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness; if (penetration < dgFloat32(0.0f)) { return 0; } dgVector p1(hullMatrix.TransformVector(hull->SupportVertex(normalInHull, NULL))); contactJoint->m_closestDistance = dgFloat32(0.0f); dgFloat32 distance = (m_localPoly[0] - p1) % m_normal; if (distance >= dgFloat32(0.0f)) { return 0; } dgVector boxSize (hull->GetBoxSize() & dgVector::m_triplexMask); dgVector boxOrigin ((hull->GetBoxOrigin() & dgVector::m_triplexMask) + dgVector::m_wOne); bool inside = true; dgInt32 i0 = m_count - 1; for (dgInt32 i = 0; i < m_count; i++) { dgVector e(m_localPoly[i] - m_localPoly[i0]); dgVector edgeBoundaryNormal(m_normal * e); dgPlane plane(edgeBoundaryNormal, - m_localPoly[i0].DotProduct4 (edgeBoundaryNormal).GetScalar()); plane = hullMatrix.TransformPlane(plane); dgFloat32 supportDist = boxSize.DotProduct4 (plane.Abs()).GetScalar(); dgFloat32 centerDist = plane.DotProduct4 (boxOrigin).GetScalar(); if ((centerDist + supportDist) < dgFloat32(0.0f)) { return 0; } if ((centerDist - supportDist) < dgFloat32(0.0f)) { inside = false; break; } i0 = i; } //inside = false; dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize(); if (m_faceClipSize > convexSphapeUmbra) { BeamClipping(dgVector(dgFloat32(0.0f)), convexSphapeUmbra); m_faceClipSize = hull->m_childShape->GetBoxMaxRadius(); } const dgInt32 hullId = hull->GetUserDataID(); if (inside & !proxy.m_intersectionTestOnly) { dgAssert(penetration >= dgFloat32(0.0f)); dgVector contactPoints[64]; dgAssert(penetration >= 0.0f); dgVector point(pointInHull + normalInHull.Scale4(penetration + DG_ROBUST_PLANE_CLIP)); count = hull->CalculatePlaneIntersection(normalInHull.Scale4(dgFloat32(-1.0f)), point, contactPoints); dgVector step(normalInHull.Scale4((proxy.m_skinThickness - penetration) * dgFloat32(0.5f))); dgContactPoint* const contactsOut = proxy.m_contacts; dgAssert(contactsOut); for (dgInt32 i = 0; i < count; i++) { contactsOut[i].m_point = hullMatrix.TransformVector(contactPoints[i] + step); contactsOut[i].m_normal = m_normal; contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; contactsOut[i].m_penetration = penetration; } } else { m_vertexCount = dgUnsigned16 (m_count); count = world->CalculateConvexToConvexContacts(proxy); dgAssert(proxy.m_intersectionTestOnly || (count >= 0)); if (count >= 1) { dgContactPoint* const contactsOut = proxy.m_contacts; if (m_closestFeatureType == 3) { for (dgInt32 i = 0; i < count; i++) { //contactsOut[i].m_userId = m_faceId; contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; } } else { dgVector normal (contactsOut[0].m_normal); if (normal.DotProduct4(m_normal).GetScalar() < dgFloat32(0.9995f)) { dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex]; dgVector adjacentNormal (CalculateGlobalNormal (parentMesh, dgVector(&m_vertex[index * m_stride]))); if ((m_normal.DotProduct4(adjacentNormal).GetScalar() > dgFloat32(0.9995f))) { normal = adjacentNormal; } else { dgVector dir0(adjacentNormal * m_normal); dgVector dir1(adjacentNormal * normal); dgFloat32 projection = dir0.DotProduct4(dir1).GetScalar(); if (projection <= dgFloat32(0.0f)) { normal = adjacentNormal; } } normal = polygonInstance->m_globalMatrix.RotateVector(normal); for (dgInt32 i = 0; i < count; i++) { contactsOut[i].m_normal = normal; contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; } } else { for (dgInt32 i = 0; i < count; i++) { contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; } } } } } return count; }
void dgCollisionSphere::Init (dgFloat32 radius, dgMemoryAllocator* allocator) { m_rtti |= dgCollisionSphere_RTTI; m_radius = radius; m_edgeCount = DG_SPHERE_EDGE_COUNT; m_vertexCount = DG_SPHERE_VERTEX_COUNT; dgCollisionConvex::m_vertex = m_vertex; if (!m_shapeRefCount) { dgInt32 indexList[256]; dgVector tmpVectex[256]; dgVector p0 ( dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); dgVector p1 (-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); dgVector p2 ( dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); dgVector p3 ( dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); dgVector p4 ( dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f)); dgVector p5 ( dgFloat32 (0.0f), dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f)); dgInt32 i = 1; dgInt32 count = 0; TesselateTriangle (i, p4, p0, p2, count, tmpVectex); TesselateTriangle (i, p4, p2, p1, count, tmpVectex); TesselateTriangle (i, p4, p1, p3, count, tmpVectex); TesselateTriangle (i, p4, p3, p0, count, tmpVectex); TesselateTriangle (i, p5, p2, p0, count, tmpVectex); TesselateTriangle (i, p5, p1, p2, count, tmpVectex); TesselateTriangle (i, p5, p3, p1, count, tmpVectex); TesselateTriangle (i, p5, p0, p3, count, tmpVectex); //dgAssert (count == EDGE_COUNT); dgInt32 vertexCount = dgVertexListToIndexList (&tmpVectex[0].m_x, sizeof (dgVector), 3 * sizeof (dgFloat32), 0, count, indexList, 0.001f); dgAssert (vertexCount == DG_SPHERE_VERTEX_COUNT); for (dgInt32 i = 0; i < vertexCount; i ++) { m_unitSphere[i] = tmpVectex[i]; } dgPolyhedra polyhedra(m_allocator); polyhedra.BeginFace(); for (dgInt32 i = 0; i < count; i += 3) { #ifdef _DEBUG dgEdge* const edge = polyhedra.AddFace (indexList[i], indexList[i + 1], indexList[i + 2]); dgAssert (edge); #else polyhedra.AddFace (indexList[i], indexList[i + 1], indexList[i + 2]); #endif } polyhedra.EndFace(); dgUnsigned64 i1 = 0; dgPolyhedra::Iterator iter (polyhedra); for (iter.Begin(); iter; iter ++) { dgEdge* const edge = &(*iter); edge->m_userData = i1; i1 ++; } for (iter.Begin(); iter; iter ++) { dgEdge* const edge = &(*iter); dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData]; ptr->m_vertex = edge->m_incidentVertex; ptr->m_next = &m_edgeArray[edge->m_next->m_userData]; ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData]; ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData]; } } for (dgInt32 i = 0; i < DG_SPHERE_VERTEX_COUNT; i ++) { m_vertex[i] = m_unitSphere[i].Scale4 (m_radius); } m_shapeRefCount ++; dgCollisionConvex::m_simplex = m_edgeArray; SetVolumeAndCG (); }
PyObject * _PyImport_LoadDynamicModuleWithSpec(PyObject *spec, FILE *fp) { #ifndef MS_WINDOWS PyObject *pathbytes = NULL; #endif PyObject *name_unicode = NULL, *name = NULL, *path = NULL, *m = NULL; const char *name_buf, *hook_prefix; const char *oldcontext; dl_funcptr exportfunc; PyModuleDef *def; PyObject *(*p0)(void); name_unicode = PyObject_GetAttrString(spec, "name"); if (name_unicode == NULL) { return NULL; } name = get_encoded_name(name_unicode, &hook_prefix); if (name == NULL) { goto error; } name_buf = PyBytes_AS_STRING(name); path = PyObject_GetAttrString(spec, "origin"); if (path == NULL) goto error; #ifdef MS_WINDOWS exportfunc = _PyImport_FindSharedFuncptrWindows(hook_prefix, name_buf, path, fp); #else pathbytes = PyUnicode_EncodeFSDefault(path); if (pathbytes == NULL) goto error; exportfunc = _PyImport_FindSharedFuncptr(hook_prefix, name_buf, PyBytes_AS_STRING(pathbytes), fp); Py_DECREF(pathbytes); #endif if (exportfunc == NULL) { if (!PyErr_Occurred()) { PyObject *msg; msg = PyUnicode_FromFormat( "dynamic module does not define " "module export function (%s_%s)", hook_prefix, name_buf); if (msg == NULL) goto error; PyErr_SetImportError(msg, name_unicode, path); Py_DECREF(msg); } goto error; } p0 = (PyObject *(*)(void))exportfunc; /* Package context is needed for single-phase init */ oldcontext = _Py_PackageContext; _Py_PackageContext = PyUnicode_AsUTF8(name_unicode); if (_Py_PackageContext == NULL) { _Py_PackageContext = oldcontext; goto error; } m = p0(); _Py_PackageContext = oldcontext; if (m == NULL) { if (!PyErr_Occurred()) { PyErr_Format( PyExc_SystemError, "initialization of %s failed without raising an exception", name_buf); } goto error; } else if (PyErr_Occurred()) { PyErr_Clear(); PyErr_Format( PyExc_SystemError, "initialization of %s raised unreported exception", name_buf); m = NULL; goto error; } if (Py_TYPE(m) == NULL) { /* This can happen when a PyModuleDef is returned without calling * PyModuleDef_Init on it */ PyErr_Format(PyExc_SystemError, "init function of %s returned uninitialized object", name_buf); m = NULL; /* prevent segfault in DECREF */ goto error; } if (PyObject_TypeCheck(m, &PyModuleDef_Type)) { Py_DECREF(name_unicode); Py_DECREF(name); Py_DECREF(path); return PyModule_FromDefAndSpec((PyModuleDef*)m, spec); } /* Fall back to single-phase init mechanism */ if (hook_prefix == nonascii_prefix) { /* don't allow legacy init for non-ASCII module names */ PyErr_Format( PyExc_SystemError, "initialization of * did not return PyModuleDef", name_buf); goto error; } /* Remember pointer to module init function. */ def = PyModule_GetDef(m); if (def == NULL) { PyErr_Format(PyExc_SystemError, "initialization of %s did not return an extension " "module", name_buf); goto error; } def->m_base.m_init = p0; /* Remember the filename as the __file__ attribute */ if (PyModule_AddObject(m, "__file__", path) < 0) PyErr_Clear(); /* Not important enough to report */ else Py_INCREF(path); if (_PyImport_FixupExtensionObject(m, name_unicode, path) < 0) goto error; Py_DECREF(name_unicode); Py_DECREF(name); Py_DECREF(path); return m; error: Py_DECREF(name_unicode); Py_XDECREF(name); Py_XDECREF(path); Py_XDECREF(m); return NULL; }
void dgCollisionTaperedCapsule::Init (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height) { m_rtti |= dgCollisionTaperedCapsule_RTTI; m_radio0 = dgAbsf (radio0); m_radio1 = dgAbsf (radio1); m_height = dgAbsf (height * 0.5f); m_clip1 = dgFloat32 (0.0f); m_clip0 = dgFloat32 (0.0f); dgFloat32 angle0 = dgFloat32 (-3.141592f / 2.0f); dgFloat32 angle1 = dgFloat32 ( 3.141592f / 2.0f); do { dgFloat32 angle = (angle1 + angle0) * dgFloat32 (0.5f); dgVector dir (dgSin (angle), dgCos (angle), dgFloat32 (0.0f), dgFloat32 (0.0f)); dgVector p0(dir.Scale3 (m_radio0)); dgVector p1(dir.Scale3 (m_radio1)); p0.m_x += m_height; p1.m_x -= m_height; dgFloat32 dir0 = p0 % dir; dgFloat32 dir1 = p1 % dir; if (dir0 > dir1) { angle1 = angle; m_clip0 = p0.m_x - m_height; } else { angle0 = angle; m_clip1 = p1.m_x + m_height; } } while ((angle1 - angle0) > dgFloat32 (0.001f * 3.141592f/180.0f)); dgFloat32 angle = (angle1 + angle0) * dgFloat32 (0.5f); m_sideNormal = dgVector (dgSin (angle), dgCos (angle), dgFloat32 (0.0f), dgFloat32 (0.0f)); m_clipRadio0 = dgSqrt (m_radio0 * m_radio0 - m_clip0 * m_clip0); m_clipRadio1 = dgSqrt (m_radio1 * m_radio1 - m_clip1 * m_clip1); dgInt32 i1 = 0; dgInt32 i0 = DG_CAPSULE_SEGMENTS * DG_CAP_SEGMENTS * 2; dgFloat32 dx0 = (m_clip0 - m_radio0) / DG_CAP_SEGMENTS; dgFloat32 x0 = m_radio0 + dx0; dgFloat32 dx1 = (m_clip1 + m_radio1) / DG_CAP_SEGMENTS; dgFloat32 x1 = -m_radio1 + dx1; for (dgInt32 j = 0; j < DG_CAP_SEGMENTS; j ++) { dgFloat32 angle = dgFloat32 (0.0f); dgFloat32 r0 = dgSqrt (m_radio0 * m_radio0 - x0 * x0); dgFloat32 r1 = dgSqrt (m_radio1 * m_radio1 - x1 * x1); i0 -= DG_CAPSULE_SEGMENTS; for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i ++) { dgFloat32 z = dgSin (angle); dgFloat32 y = dgCos (angle); m_vertex[i0] = dgVector ( m_height + x0, y * r0, z * r0, dgFloat32 (0.0f)); m_vertex[i1] = dgVector (-m_height + x1, y * r1, z * r1, dgFloat32 (0.0f)); i0 ++; i1 ++; angle += dgPI2 / DG_CAPSULE_SEGMENTS; } x0 += dx0; x1 += dx1; i0 -= DG_CAPSULE_SEGMENTS; } m_vertexCount = DG_CAPSULE_SEGMENTS * DG_CAP_SEGMENTS * 2; m_edgeCount = DG_CAPSULE_SEGMENTS * (6 + 8 * (DG_CAP_SEGMENTS - 1)); dgCollisionConvex::m_vertex = m_vertex; if (!m_shapeRefCount) { dgPolyhedra polyhedra(m_allocator); dgInt32 wireframe[DG_CAPSULE_SEGMENTS + 10]; i1 = 0; i0 = DG_CAPSULE_SEGMENTS - 1; polyhedra.BeginFace (); for (dgInt32 j = 0; j < DG_CAP_SEGMENTS * 2 - 1; j ++) { for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i ++) { wireframe[0] = i0; wireframe[1] = i1; wireframe[2] = i1 + DG_CAPSULE_SEGMENTS; wireframe[3] = i0 + DG_CAPSULE_SEGMENTS; i0 = i1; i1 ++; polyhedra.AddFace (4, wireframe); } i0 = i1 + DG_CAPSULE_SEGMENTS - 1; } for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i ++) { wireframe[i] = DG_CAPSULE_SEGMENTS - 1 - i; } polyhedra.AddFace (DG_CAPSULE_SEGMENTS, wireframe); for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i ++) { wireframe[i] = i + DG_CAPSULE_SEGMENTS * (DG_CAP_SEGMENTS * 2 - 1); } polyhedra.AddFace (DG_CAPSULE_SEGMENTS, wireframe); polyhedra.EndFace (); dgAssert (SanityCheck (polyhedra)); dgUnsigned64 i = 0; dgPolyhedra::Iterator iter (polyhedra); for (iter.Begin(); iter; iter ++) { dgEdge* const edge = &(*iter); edge->m_userData = i; i ++; } for (iter.Begin(); iter; iter ++) { dgEdge* const edge = &(*iter); dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData]; ptr->m_vertex = edge->m_incidentVertex; ptr->m_next = &m_edgeArray[edge->m_next->m_userData]; ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData]; ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData]; } } m_shapeRefCount ++; dgCollisionConvex::m_simplex = m_edgeArray; SetVolumeAndCG (); }
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy) { dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI)); dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI)); dgAssert(this == proxy.m_instance1->GetChildShape()); dgAssert(m_count); dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0]))); const dgBody* const body0 = proxy.m_body0; const dgBody* const body1 = proxy.m_body1; dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity()); dgVector relativeVelocity (body0->m_veloc - body1->m_veloc); if (m_normal.DotProduct4(relativeVelocity).GetScalar() >= 0.0f) { return 0; } dgFloat32 den = dgFloat32 (1.0f) / (relativeVelocity % m_normal); if (den > dgFloat32 (1.0e-5f)) { // this can actually happens dgAssert(0); return 0; } dgContact* const contactJoint = proxy.m_contactJoint; contactJoint->m_closestDistance = dgFloat32(1.0e10f); dgMatrix polygonMatrix; dgVector right (m_localPoly[1] - m_localPoly[0]); polygonMatrix[0] = right.CompProduct4(right.InvMagSqrt()); polygonMatrix[1] = m_normal; polygonMatrix[2] = polygonMatrix[0] * m_normal; polygonMatrix[3] = dgVector::m_wOne; dgAssert (polygonMatrix.TestOrthogonal()); dgVector polyBoxP0(dgFloat32(1.0e15f)); dgVector polyBoxP1(dgFloat32(-1.0e15f)); for (dgInt32 i = 0; i < m_count; i++) { dgVector point (polygonMatrix.UnrotateVector(m_localPoly[i])); polyBoxP0 = polyBoxP0.GetMin(point); polyBoxP1 = polyBoxP1.GetMax(point); } dgVector hullBoxP0; dgVector hullBoxP1; dgMatrix hullMatrix (polygonMatrix * proxy.m_instance0->m_globalMatrix); proxy.m_instance0->CalcAABB(hullMatrix, hullBoxP0, hullBoxP1); dgVector minBox(polyBoxP0 - hullBoxP1); dgVector maxBox(polyBoxP1 - hullBoxP0); dgVector veloc (polygonMatrix.UnrotateVector (relativeVelocity)); dgFastRayTest ray(dgVector(dgFloat32(0.0f)), veloc); dgFloat32 distance = ray.BoxIntersect(minBox, maxBox); dgInt32 count = 0; if (distance < dgFloat32(1.0f)) { bool inside = false; dgVector boxSize((hullBoxP1 - hullBoxP0).CompProduct4(dgVector::m_half)); dgVector sphereMag2 (boxSize.DotProduct4(boxSize)); boxSize = sphereMag2.Sqrt(); dgVector pointInPlane (polygonMatrix.RotateVector(hullBoxP1 + hullBoxP0).CompProduct4(dgVector::m_half)); dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal; dgFloat32 timeToPlane0 = (distToPlane + boxSize.GetScalar()) * den; dgFloat32 timeToPlane1 = (distToPlane - boxSize.GetScalar()) * den; dgVector boxOrigin0 (pointInPlane + relativeVelocity.Scale4(timeToPlane0)); dgVector boxOrigin1 (pointInPlane + relativeVelocity.Scale4(timeToPlane1)); dgVector boxOrigin ((boxOrigin0 + boxOrigin1).CompProduct4(dgVector::m_half)); dgVector boxProjectSize (((boxOrigin0 - boxOrigin1).CompProduct4(dgVector::m_half))); sphereMag2 = boxProjectSize.DotProduct4(boxProjectSize); boxSize = sphereMag2.Sqrt(); dgAssert (boxOrigin.m_w == 0.0f); boxOrigin = boxOrigin | dgVector::m_wOne; if (!proxy.m_intersectionTestOnly) { inside = true; dgInt32 i0 = m_count - 1; for (dgInt32 i = 0; i < m_count; i++) { dgVector e(m_localPoly[i] - m_localPoly[i0]); dgVector n(m_normal * e & dgVector::m_triplexMask); dgFloat32 param = dgSqrt (sphereMag2.GetScalar() / (n.DotProduct4(n)).GetScalar()); dgPlane plane(n, -(m_localPoly[i0] % n)); dgVector p0 (boxOrigin + n.Scale4 (param)); dgVector p1 (boxOrigin - n.Scale4 (param)); dgFloat32 size0 = (plane.DotProduct4 (p0)).GetScalar(); dgFloat32 size1 = (plane.DotProduct4 (p1)).GetScalar(); if ((size0 < 0.0f) && (size1 < 0.0f)) { return 0; } if ((size0 * size1) < 0.0f) { inside = false; break; } i0 = i; } } dgFloat32 convexSphapeUmbra = dgMax (proxy.m_instance0->GetUmbraClipSize(), boxSize.GetScalar()); if (m_faceClipSize > convexSphapeUmbra) { BeamClipping(boxOrigin, convexSphapeUmbra); m_faceClipSize = proxy.m_instance0->m_childShape->GetBoxMaxRadius(); } const dgInt32 hullId = proxy.m_instance0->GetUserDataID(); if (inside & !proxy.m_intersectionTestOnly) { const dgMatrix& matrixInstance0 = proxy.m_instance0->m_globalMatrix; dgVector normalInHull(matrixInstance0.UnrotateVector(m_normal.Scale4(dgFloat32(-1.0f)))); dgVector pointInHull(proxy.m_instance0->SupportVertex(normalInHull, NULL)); dgVector p0 (matrixInstance0.TransformVector(pointInHull)); dgFloat32 timetoImpact = dgFloat32(0.0f); dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness; if (penetration < dgFloat32(0.0f)) { timetoImpact = penetration / (relativeVelocity % m_normal); dgAssert(timetoImpact >= dgFloat32(0.0f)); } if (timetoImpact <= proxy.m_timestep) { dgVector contactPoints[64]; contactJoint->m_closestDistance = penetration; proxy.m_timestep = timetoImpact; proxy.m_normal = m_normal; proxy.m_closestPointBody0 = p0; proxy.m_closestPointBody1 = p0 + m_normal.Scale4(penetration); if (!proxy.m_intersectionTestOnly) { pointInHull -= normalInHull.Scale4 (DG_ROBUST_PLANE_CLIP); count = proxy.m_instance0->CalculatePlaneIntersection(normalInHull, pointInHull, contactPoints); dgVector step(relativeVelocity.Scale4(timetoImpact)); penetration = dgMax(penetration, dgFloat32(0.0f)); dgContactPoint* const contactsOut = proxy.m_contacts; for (dgInt32 i = 0; i < count; i++) { contactsOut[i].m_point = matrixInstance0.TransformVector(contactPoints[i]) + step; contactsOut[i].m_normal = m_normal; contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; contactsOut[i].m_penetration = penetration; } } } } else { m_vertexCount = dgUnsigned16 (m_count); count = world->CalculateConvexToConvexContacts(proxy); if (count >= 1) { dgContactPoint* const contactsOut = proxy.m_contacts; for (dgInt32 i = 0; i < count; i++) { contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; } } } } return count; }
// 随机模型1 Model randomModel(double length, double width, double margin, double sMaxH, double sMinH, double sMaxW, double sMinW, double surfaceInterval, double vRange, int dFlag) { Direction* tdp; Direction* rdp; switch(dFlag) { case 1: tdp = new Direction(0, 1, 0); rdp = new Direction(0, 1, 0); break; case 2: tdp = new Direction(0, -1, 0); rdp = new Direction(0, -1, 0); break; case 3: tdp = new Direction(0, 1, 0); rdp = new Direction(0, -1, 0); break; case 4: tdp = new Direction(0, -1, 0); rdp = new Direction(0, 1, 0); break; default: double ty = plusOrMinus(); double ry = plusOrMinus(); Direction td(0, ty, 0); Direction rd(0, ry, 0); break; } Vehicle tx = randomVehicle(0, width, *tdp, vRange); Vehicle rx = randomVehicle(length, width, *rdp, vRange); vector<Material> materials = materialList(); vector<Surface> surfaces; double totalLength = 0; srand((int)time(0)); while (totalLength <= length) { double x0 = (width / 2) + random((int)margin); double y0 = totalLength; double z0 = sMinH + random(int(sMaxH - sMinH)); Point p0(x0, y0, z0); double sWidth = sMinW + random((int)(sMaxW - sMinW)); double x1 = x0; double y1 = y0 + sWidth; double z1 = 0; Point p1(x1, y1, z1); Surface s(p0, p1); surfaces.push_back(s); totalLength = y0 + sWidth + random((int)surfaceInterval); } totalLength = 0; while (totalLength <= length) { double x0 = -((width / 2) + random((int)margin)); double y0 = totalLength; double z0 = sMinH + random(int(sMaxH - sMinH)); Point p0(x0, y0, z0); double sWidth = sMinW + random((int)(sMaxW - sMinW)); double x1 = x0; double y1 = y0 + sWidth; double z1 = 0; Point p1(x1, y1, z1); Surface s(p0, p1); surfaces.push_back(s); totalLength = y0 + sWidth + random((int)surfaceInterval); } int sizeM = materials.size(); for (int i = 0; i < (int)surfaces.size(); i++) { int indexM = random(sizeM); surfaces[i].setMaterial(materials[indexM]); surfaces[i].init(); } Direction tad(0,0,1); Direction tarp(0,0,0); Direction taPolar(sqrt(double(2))/2, 0, sqrt(double(2))/2); Antenna ta(tad, tarp, taPolar, 10); vector<Antenna> tas; tas.push_back(ta); Direction rad(0,0,1); Direction rarp(0,0,0); Direction raVPolar(1, 0, 0); Direction raHPolar(0, 0, 1); Antenna ra(rad, rarp, raVPolar, raHPolar); vector<Antenna> ras; ras.push_back(ra); Model model(tx, rx, surfaces, tas, ras); return model; }
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue (dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale) { dgAssert (proxy.m_referenceCollision->IsType (dgCollision::dgCollisionConvexShape_RTTI)); dgAssert (proxy.m_floatingCollision->IsType (dgCollision::dgCollisionConvexPolygon_RTTI)); const dgCollisionInstance* const hull = proxy.m_referenceCollision; dgAssert (this == proxy.m_floatingCollision->GetChildShape()); dgAssert (m_count); dgAssert (m_count < dgInt32 (sizeof (m_localPoly) / sizeof (m_localPoly[0]))); const dgBody* const floatingBody = proxy.m_floatingBody; const dgBody* const referenceBody = proxy.m_referenceBody; dgContact* const contactJoint = proxy.m_contactJoint; contactJoint->m_closestDistance = dgFloat32 (1.0e10f); m_normal = m_normal.CompProduct4(polyInstanceInvScale); dgAssert (m_normal.m_w == dgFloat32 (0.0f)); m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt()); const dgVector savedFaceNormal (m_normal); for (dgInt32 i = 0; i < m_count; i ++) { m_localPoly[i] = polyInstanceScale.CompProduct4(dgVector (&m_vertex[m_vertexIndex[i] * m_stride])); dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f)); } dgVector hullOrigin (proxy.m_matrix.UntransformVector(dgVector (dgFloat32 (0.0f)))); hullOrigin = (hullOrigin - m_normal.CompProduct4(m_normal.DotProduct4(hullOrigin - m_localPoly[0]))) | dgVector::m_wOne; dgMatrix polygonMatrix; polygonMatrix[0] = m_localPoly[1] - m_localPoly[0]; polygonMatrix[0] = polygonMatrix[0].CompProduct4 (polygonMatrix[0].InvMagSqrt()); polygonMatrix[1] = m_normal; polygonMatrix[2] = polygonMatrix[0] * m_normal; polygonMatrix[3] = hullOrigin; dgAssert (polygonMatrix.TestOrthogonal()); dgMatrix savedProxyMatrix (proxy.m_matrix); proxy.m_matrix = polygonMatrix * proxy.m_matrix; dgVector floatingVeloc (floatingBody->m_veloc); dgVector referenceVeloc (referenceBody->m_veloc); const dgMatrix& hullMatrix = hull->GetGlobalMatrix(); dgVector hullRelativeVeloc (hullMatrix.UnrotateVector(referenceVeloc - floatingVeloc)); dgVector polyRelativeVeloc (proxy.m_matrix.UnrotateVector (hullRelativeVeloc)); dgVector polyBoxP0 (dgFloat32 ( 1.0e15f)); dgVector polyBoxP1 (dgFloat32 (-1.0e15f)); m_normal = polygonMatrix.UnrotateVector(m_normal); if (m_normal.DotProduct4(polyRelativeVeloc).m_x >= 0.0f) { proxy.m_matrix = savedProxyMatrix; return 0; } for (dgInt32 i = 0; i < m_count; i ++) { m_localPoly[i] = polygonMatrix.UntransformVector(m_localPoly[i]); dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f)); polyBoxP0 = polyBoxP0.GetMin (m_localPoly[i]); polyBoxP1 = polyBoxP1.GetMax (m_localPoly[i]); } dgInt32 count = 0; dgVector hullBoxP0; dgVector hullBoxP1; hull->CalcAABB (proxy.m_matrix.Inverse(), hullBoxP0, hullBoxP1); dgVector minBox (polyBoxP0 - hullBoxP1); dgVector maxBox (polyBoxP1 - hullBoxP0); dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), polyRelativeVeloc); dgFloat32 distance = ray.BoxIntersect(minBox, maxBox); if (distance < dgFloat32 (1.0f)) { dgVector boxSize ((hullBoxP1 - hullBoxP0).Scale4 (dgFloat32 (0.5f))); // dgVector boxOrigin ((hullBoxP1 + hullBoxP0).Scale4 (dgFloat32 (0.5f))); // boxOrigin += polyRelativeVeloc.Scale4 (distance); dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f)))); dgVector pointInHull (hull->SupportVertex (normalInHull, NULL)); dgVector pointInPlane (proxy.m_matrix.UntransformVector (pointInHull)); dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal; dgFloat32 timeToPlane = distToPlane / (polyRelativeVeloc % m_normal); dgVector boxOrigin (pointInPlane + polyRelativeVeloc.Scale4(timeToPlane)); bool inside = true; dgInt32 i0 = m_count - 1; for (dgInt32 i = 0; i < m_count; i ++) { dgVector e (m_localPoly[i] - m_localPoly[i0]); dgVector n (m_normal * e); dgPlane plane (n, - (m_localPoly[i0] % n)); dgVector supportDist (plane.Abs().DotProduct4 (boxSize)); dgFloat32 centerDist = plane.Evalue(boxOrigin); if ((centerDist + supportDist.m_x) < dgFloat32 (0.0f)) { proxy.m_matrix = savedProxyMatrix; return 0; } if ((centerDist - supportDist.m_x) < dgFloat32 (0.0f)) { inside = false; } i0 = i; } // for the time being for the minkousky contact calculation inside = false; const dgInt32 hullId = hull->GetUserDataID(); if (inside) { dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f)))); dgVector pointInHull (hull->SupportVertex (normalInHull, NULL)); dgVector p0 (proxy.m_matrix.UntransformVector (pointInHull)); dgFloat32 timetoImpact = dgFloat32 (0.0f); //dgFloat32 closestDistance = dgFloat32 (0.0f); dgAssert (0); // dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness + DG_IMPULSIVE_CONTACT_PENETRATION; dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness; if (penetration < dgFloat32 (0.0f)) { timetoImpact = penetration / (polyRelativeVeloc % m_normal); dgAssert (timetoImpact >= dgFloat32 (0.0f)); // closestDistance = -penetration; } if (timetoImpact <= proxy.m_timestep) { dgVector pointsContacts[64]; contactJoint->m_closestDistance = penetration; dgAssert (0); // dgVector point (pointInHull - normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION)); dgVector point (pointInHull); count = hull->CalculatePlaneIntersection (normalInHull, point, pointsContacts, 1.0f); dgAssert (0); // dgVector step (hullRelativeVeloc.Scale3 (timetoImpact) + normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION)); dgVector step (hullRelativeVeloc.Scale3 (timetoImpact)); penetration = dgMax (penetration, dgFloat32 (0.0f)); const dgMatrix& worldMatrix = hull->m_globalMatrix; dgContactPoint* const contactsOut = proxy.m_contacts; dgVector globalNormal (worldMatrix.RotateVector(normalInHull)); for (dgInt32 i = 0; i < count; i ++) { contactsOut[i].m_point = worldMatrix.TransformVector (pointsContacts[i] + step); contactsOut[i].m_normal = globalNormal; contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; contactsOut[i].m_penetration = penetration; } } } else { dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize (); if (m_faceClipSize > convexSphapeUmbra) { BeamClipping (boxOrigin, convexSphapeUmbra); m_faceClipSize = hull->m_childShape->GetBoxMaxRadius(); } dgCollisionConvex* const convexShape = (dgCollisionConvex*) hull->m_childShape; count = convexShape->CalculateConvexCastContacts (proxy); // dgAssert (proxy.m_intersectionTestOnly || (count >= 0)); if (count >= 1) { dgContactPoint* const contactsOut = proxy.m_contacts; #if 0 if (m_closestFeatureType == 3) { for (dgInt32 i = 0; i < count; i ++) { contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; } } else { dgVector normal (polygonInstance->m_globalMatrix.UnrotateVector(contactsOut[0].m_normal)); if ((normal % savedFaceNormal) < dgFloat32 (0.995f)) { dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex]; dgVector n (&m_vertex[index * m_stride]); dgVector dir0 (n * savedFaceNormal); dgVector dir1 (n * normal); dgFloat32 projection = dir0 % dir1; if (projection <= dgFloat32 (0.0f)) { normal = n; } normal = polygonInstance->m_globalMatrix.RotateVector(normal); for (dgInt32 i = 0; i < count; i ++) { contactsOut[i].m_normal = normal; //contactsOut[i].m_userId = m_faceId; contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; } } else { for (dgInt32 i = 0; i < count; i ++) { //contactsOut[i].m_userId = m_faceId; contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; } } } #endif for (dgInt32 i = 0; i < count; i ++) { contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; } } } } proxy.m_matrix = savedProxyMatrix; return count; }
void Path::addArcTo(const FloatPoint& p1, const FloatPoint& p2, float radius) { FloatPoint p0(m_path.currentPosition()); if ((p1.x() == p0.x() && p1.y() == p0.y()) || (p1.x() == p2.x() && p1.y() == p2.y()) || radius == 0.f) { m_path.lineTo(p1); return; } FloatPoint p1p0((p0.x() - p1.x()), (p0.y() - p1.y())); FloatPoint p1p2((p2.x() - p1.x()), (p2.y() - p1.y())); float p1p0_length = sqrtf(p1p0.x() * p1p0.x() + p1p0.y() * p1p0.y()); float p1p2_length = sqrtf(p1p2.x() * p1p2.x() + p1p2.y() * p1p2.y()); double cos_phi = (p1p0.x() * p1p2.x() + p1p0.y() * p1p2.y()) / (p1p0_length * p1p2_length); // The points p0, p1, and p2 are on the same straight line (HTML5, 4.8.11.1.8) // We could have used areCollinear() here, but since we're reusing // the variables computed above later on we keep this logic. if (qFuzzyCompare(qAbs(cos_phi), 1.0)) { m_path.lineTo(p1); return; } float tangent = radius / tan(acos(cos_phi) / 2); float factor_p1p0 = tangent / p1p0_length; FloatPoint t_p1p0((p1.x() + factor_p1p0 * p1p0.x()), (p1.y() + factor_p1p0 * p1p0.y())); FloatPoint orth_p1p0(p1p0.y(), -p1p0.x()); float orth_p1p0_length = sqrt(orth_p1p0.x() * orth_p1p0.x() + orth_p1p0.y() * orth_p1p0.y()); float factor_ra = radius / orth_p1p0_length; // angle between orth_p1p0 and p1p2 to get the right vector orthographic to p1p0 double cos_alpha = (orth_p1p0.x() * p1p2.x() + orth_p1p0.y() * p1p2.y()) / (orth_p1p0_length * p1p2_length); if (cos_alpha < 0.f) orth_p1p0 = FloatPoint(-orth_p1p0.x(), -orth_p1p0.y()); FloatPoint p((t_p1p0.x() + factor_ra * orth_p1p0.x()), (t_p1p0.y() + factor_ra * orth_p1p0.y())); // calculate angles for addArc orth_p1p0 = FloatPoint(-orth_p1p0.x(), -orth_p1p0.y()); float sa = acos(orth_p1p0.x() / orth_p1p0_length); if (orth_p1p0.y() < 0.f) sa = 2 * piDouble - sa; // anticlockwise logic bool anticlockwise = false; float factor_p1p2 = tangent / p1p2_length; FloatPoint t_p1p2((p1.x() + factor_p1p2 * p1p2.x()), (p1.y() + factor_p1p2 * p1p2.y())); FloatPoint orth_p1p2((t_p1p2.x() - p.x()), (t_p1p2.y() - p.y())); float orth_p1p2_length = sqrtf(orth_p1p2.x() * orth_p1p2.x() + orth_p1p2.y() * orth_p1p2.y()); float ea = acos(orth_p1p2.x() / orth_p1p2_length); if (orth_p1p2.y() < 0) ea = 2 * piDouble - ea; if ((sa > ea) && ((sa - ea) < piDouble)) anticlockwise = true; if ((sa < ea) && ((ea - sa) > piDouble)) anticlockwise = true; m_path.lineTo(t_p1p0); addArc(p, radius, sa, ea, anticlockwise); }
int main() { Triangulation t; Face_handle fh; // Check the empty triangulation fh = test_point_location(t, Point(0.5, 0.5), Triangulation::EMPTY); CGAL_assertion(fh == Face_handle()); // Insert the first point Point p0(0.5, 0.5); Vertex_handle vh0 = t.insert(p0); CGAL_assertion(t.is_valid(true)); CGAL_USE(vh0); fh = test_point_location(t, p0, Triangulation::VERTEX); CGAL_assertion(fh->has_vertex(vh0)); fh = test_point_location(t, p0 + Vector(0.1, 0.1), Triangulation::EDGE); CGAL_assertion(fh->has_vertex(vh0)); fh = test_point_location(t, p0 + Vector(-0.1, -0.1), Triangulation::EDGE); CGAL_assertion(fh->has_vertex(vh0)); fh = test_point_location(t, p0 + Vector(-0.2, -0.3), Triangulation::FACE); CGAL_assertion(fh->has_vertex(vh0)); CGAL_assertion(t.is_valid(true)); // Insert the second point on an edge Point p1(0.7, 0.7); Vertex_handle vh1 = t.insert(p1); CGAL_USE(vh1); CGAL_assertion(t.is_valid(true)); fh = test_point_location(t, p0, Triangulation::VERTEX); CGAL_assertion(fh->has_vertex(vh0)); fh = test_point_location(t, p1, Triangulation::VERTEX); CGAL_assertion(fh->has_vertex(vh1)); fh = test_point_location(t, p0 + Vector(0.1, 0.1), Triangulation::EDGE); CGAL_assertion(fh->has_vertex(vh0)); CGAL_assertion(fh->has_vertex(vh1)); fh = test_point_location(t, p0 + Vector(-0.1, -0.1), Triangulation::EDGE); CGAL_assertion(fh->has_vertex(vh0)); CGAL_assertion(!fh->has_vertex(vh1)); fh = test_point_location(t, p1 + Vector(0.1, 0.1), Triangulation::EDGE); CGAL_assertion(!fh->has_vertex(vh0)); CGAL_assertion(fh->has_vertex(vh1)); fh = test_point_location(t, p0 + Vector(-0.02, -0.03), Triangulation::FACE); CGAL_assertion(fh->has_vertex(vh0)); fh = test_point_location(t, p1 + Vector(-0.02, -0.03), Triangulation::FACE); CGAL_assertion(fh->has_vertex(vh1)); CGAL_assertion(t.is_valid(true)); // Insert the third point in a face Point p2(0.8, 0.6); Vertex_handle vh2 = t.insert(p2); CGAL_USE(vh2); CGAL_assertion(t.is_valid(true)); fh = test_point_location(t, p0, Triangulation::VERTEX); CGAL_assertion(fh->has_vertex(vh0)); fh = test_point_location(t, p1, Triangulation::VERTEX); CGAL_assertion(fh->has_vertex(vh1)); fh = test_point_location(t, p2, Triangulation::VERTEX); CGAL_assertion(fh->has_vertex(vh2)); fh = test_point_location(t, Point(0.6, 0.6), Triangulation::EDGE); CGAL_assertion(fh->has_vertex(vh0)); CGAL_assertion(fh->has_vertex(vh1)); test_point_location(t, Point(0.7, 0.6), Triangulation::FACE); test_point_location(t, p0 + Vector(-0.02, -0.03), Triangulation::FACE); test_point_location(t, p0 + Vector(0.02, -0.03), Triangulation::FACE); test_point_location(t, p0 + Vector(-0.02, 0.03), Triangulation::FACE); test_point_location(t, p0 + Vector(0.02, 0.03), Triangulation::FACE); return 0; }
dgUnsigned32 dgBallConstraint::JacobianDerivative(dgContraintDescritor& params) { dgInt32 ret; dgFloat32 relVelocErr; dgFloat32 penetrationErr; dgMatrix matrix0; dgMatrix matrix1; if (m_jointUserCallback) { m_jointUserCallback(*this, params.m_timestep); } dgVector angle(CalculateGlobalMatrixAndAngle(matrix0, matrix1)); m_angles = angle.Scale(-dgFloat32(1.0f)); const dgVector& dir0 = matrix0.m_front; const dgVector& dir1 = matrix0.m_up; const dgVector& dir2 = matrix0.m_right; const dgVector& p0 = matrix0.m_posit; const dgVector& p1 = matrix1.m_posit; dgPointParam pointData; InitPointParam(pointData, m_stiffness, p0, p1); CalculatePointDerivative(0, params, dir0, pointData, &m_jointForce[0]); CalculatePointDerivative(1, params, dir1, pointData, &m_jointForce[1]); CalculatePointDerivative(2, params, dir2, pointData, &m_jointForce[2]); ret = 3; if (m_twistLimit) { if (angle.m_x > m_twistAngle) { dgVector p0(matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH)); InitPointParam(pointData, m_stiffness, p0, p0); const dgVector& dir = matrix0.m_right; CalculatePointDerivative(ret, params, dir, pointData, &m_jointForce[ret]); dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0); relVelocErr = velocError % dir; if (relVelocErr > dgFloat32(1.0e-3f)) { relVelocErr *= dgFloat32(1.1f); } penetrationErr = MIN_JOINT_PIN_LENGTH * (angle.m_x - m_twistAngle); _ASSERTE(penetrationErr >= dgFloat32 (0.0f)); params.m_forceBounds[ret].m_low = dgFloat32(0.0f); params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT; params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret]; // params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep; SetMotorAcceleration(ret, (relVelocErr + penetrationErr) * params.m_invTimestep, params); ret++; } else if (angle.m_x < -m_twistAngle) { dgVector p0(matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH)); InitPointParam(pointData, m_stiffness, p0, p0); dgVector dir(matrix0.m_right.Scale(-dgFloat32(1.0f))); CalculatePointDerivative(ret, params, dir, pointData, &m_jointForce[ret]); dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0); relVelocErr = velocError % dir; if (relVelocErr > dgFloat32(1.0e-3f)) { relVelocErr *= dgFloat32(1.1f); } penetrationErr = MIN_JOINT_PIN_LENGTH * (-m_twistAngle - angle.m_x); _ASSERTE(penetrationErr >= dgFloat32 (0.0f)); params.m_forceBounds[ret].m_low = dgFloat32(0.0f); params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT; params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret]; // params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep; SetMotorAcceleration(ret, (relVelocErr + penetrationErr) * params.m_invTimestep, params); ret++; } } if (m_coneLimit) { dgFloat32 coneCos; coneCos = matrix0.m_front % matrix1.m_front; if (coneCos < m_coneAngleCos) { dgVector p0( matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH)); InitPointParam(pointData, m_stiffness, p0, p0); dgVector tangentDir(matrix0.m_front * matrix1.m_front); tangentDir = tangentDir.Scale( dgRsqrt ((tangentDir % tangentDir) + 1.0e-8f)); CalculatePointDerivative(ret, params, tangentDir, pointData, &m_jointForce[ret]); ret++; dgVector normalDir(tangentDir * matrix0.m_front); dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0); //restitution = contact.m_restitution; relVelocErr = velocError % normalDir; if (relVelocErr > dgFloat32(1.0e-3f)) { relVelocErr *= dgFloat32(1.1f); } penetrationErr = MIN_JOINT_PIN_LENGTH * (dgAcos (GetMax (coneCos, dgFloat32(-0.9999f))) - m_coneAngle); _ASSERTE(penetrationErr >= dgFloat32 (0.0f)); CalculatePointDerivative(ret, params, normalDir, pointData, &m_jointForce[ret]); params.m_forceBounds[ret].m_low = dgFloat32(0.0f); params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT; params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret]; // params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep; SetMotorAcceleration(ret, (relVelocErr + penetrationErr) * params.m_invTimestep, params); ret++; } } return dgUnsigned32(ret); }
void Framework::update(){ sleep( 16 ); if ( gFirstFrame ){ gFirstFrame = false; gImage = new Image( "background.dds" ); gImageWidth = gImage->width(); gImageHeight = gImage->height(); createTexture( &gTexture, gImageWidth, gImageHeight, gImage->data(), gImageWidth, gImageHeight ); SAFE_DELETE( gImage ); } setTexture( gTexture ); double rotation = static_cast< double >( gCount ); Vector2 scale( sin( rotation ) + 0.5, cos( rotation ) + 0.5 ); //行列作る Matrix23 m; m.setTranslation( Vector2( gImageWidth/2, gImageHeight/2 ) ); if ( gScaleFirst ){ m.rotate( rotation ); m.scale( scale ); }else{ m.scale( scale ); m.rotate( rotation ); } m.translate( Vector2( -gImageWidth/2, -gImageHeight/2 ) ); Vector2 p0( 0.0, 0.0 ); Vector2 p1( 100.0, 0.0 ); Vector2 p2( 0.0, 100.0 ); Vector2 p3( 100.0, 100.0 ); double t0[ 2 ] = { 0.0, 0.0 }; double t1[ 2 ] = { 1.0, 0.0 }; double t2[ 2 ] = { 0.0, 1.0 }; double t3[ 2 ] = { 1.0, 1.0 }; //行列乗算 m.multiply( &p0, p0 ); m.multiply( &p1, p1 ); m.multiply( &p2, p2 ); m.multiply( &p3, p3 ); //描画 //Vector2はそのままは渡せません。 drawTriangle2D( &p0.x, &p1.x, &p2.x, t0, t1, t2 ); //012 drawTriangle2D( &p3.x, &p1.x, &p2.x, t3, t1, t2 ); //312 //スペースで切り替え if ( Input::Manager::instance().keyboard().isTriggered( ' ' ) ){ gScaleFirst = !gScaleFirst; } ++gCount; drawDebugString( 0, 0, "press SPACE to swap ROTATION and SCALING" ); //終了処理してみようか if ( isEndRequested() ){ destroyTexture( &gTexture ); } }
void manifold(Mesh<Vertex3d> &mesh) { const int N = 11; const int M = N - 1; const double L = ((double)N) / 2.0; Vertex3d vertices[N * N]; int indices[M * M * 6]; int index = 0; for (int ix = 0; ix < N; ++ix) { for (int iy = 0; iy < N; ++iy) { double x = (1.0 * ix - L + 0.5) / L; double y = (1.0 * iy - L + 0.5) / L; double z = manifoldFunction(x,y); double xdx = x + 0.01; double ydy = y + 0.01; double zdx = manifoldFunction(xdx, y); double zdy = manifoldFunction(x, ydy); GAL::P3d p0(x,y,z); GAL::P3d pdx(xdx, y, zdx); GAL::P3d pdy(x, ydy, zdy); GAL::P3d nor = GAL::Cross(pdx - p0, pdy - p0); vertices[index].position = p0; vertices[index].normal = nor / GAL::Len(nor); ++index; } } index = 0; for (int n = 0; n < M; ++n) { for (int m = 0; m < M; ++m) { int i0 = n * N + m; int i1 = i0 + 1; int i2 = i0 + N; int i3 = i0 + N + 1; indices[index++] = i0; indices[index++] = i2; indices[index++] = i1; indices[index++] = i1; indices[index++] = i2; indices[index++] = i3; } } mesh.addVertices(vertices, sizeof(vertices) / sizeof(GAL::P3d)); mesh.addIndices(indices, sizeof(indices) / sizeof(int)); }
void Render(DemoEntityManager* const scene) { NewtonCollision* const deformableCollision = NewtonBodyGetCollision(m_body); dAssert((NewtonCollisionGetType(deformableCollision) == SERIALIZE_ID_CLOTH_PATCH) || (NewtonCollisionGetType(deformableCollision) == SERIALIZE_ID_DEFORMABLE_SOLID)); const dFloat* const particles = NewtonDeformableMeshGetParticleArray(deformableCollision); int stride = NewtonDeformableMeshGetParticleStrideInBytes(deformableCollision) / sizeof (dFloat); // calculate vertex skinning for (int i = 0; i < m_vertexCount; i++) { int index = m_indexMap[i] * stride; m_vertex[i * 3 + 0] = particles[index + 0]; m_vertex[i * 3 + 1] = particles[index + 1]; m_vertex[i * 3 + 2] = particles[index + 2]; // clear the normal for next loop m_normal[i * 3 + 0] = 0.0f; m_normal[i * 3 + 1] = 0.0f; m_normal[i * 3 + 2] = 0.0f; } // calculate vertex normals int normalStride = 3; for (DemoMesh::dListNode* segmentNode = GetFirst(); segmentNode; segmentNode = segmentNode->GetNext()) { const DemoSubMesh& subSegment = segmentNode->GetInfo(); for (int i = 0; i < subSegment.m_indexCount; i += 3) { int i0 = subSegment.m_indexes[i + 0] * normalStride; int i1 = subSegment.m_indexes[i + 1] * normalStride; int i2 = subSegment.m_indexes[i + 2] * normalStride; dVector p0(m_vertex[i0], m_vertex[i0 + 1], m_vertex[i0 + 2], 0.0f); dVector p1(m_vertex[i1], m_vertex[i1 + 1], m_vertex[i1 + 2], 0.0f); dVector p2(m_vertex[i2], m_vertex[i2 + 1], m_vertex[i2 + 2], 0.0f); dVector p10(p1 - p0); dVector p20(p2 - p0); dVector normal(p10.CrossProduct(p20)); normal = normal.Scale(1.0f / dSqrt(normal.DotProduct3(normal))); m_normal[i0 + 0] += normal.m_x; m_normal[i0 + 1] += normal.m_y; m_normal[i0 + 2] += normal.m_z; m_normal[i1 + 0] += normal.m_x; m_normal[i1 + 1] += normal.m_y; m_normal[i1 + 2] += normal.m_z; m_normal[i2 + 0] += normal.m_x; m_normal[i2 + 1] += normal.m_y; m_normal[i2 + 2] += normal.m_z; } } // normalize all the normals for (int i = 0; i < m_vertexCount; i++) { dVector n(m_normal[i * 3 + 0], m_normal[i * 3 + 1], m_normal[i * 3 + 2], 0.0f); n = n.Scale(1.0f / dSqrt(n.DotProduct3(n))); m_normal[i * 3 + 0] = n.m_x; m_normal[i * 3 + 1] = n.m_y; m_normal[i * 3 + 2] = n.m_z; } glDisable(GL_CULL_FACE); DemoMesh::Render(scene); glEnable(GL_CULL_FACE); }
static void Statistics (dgSphere &sphere, dgVector &eigenValues, dgVector &scaleVector, const hacd::HaF32 vertex[], const hacd::HaI32 faceIndex[], hacd::HaI32 indexCount, hacd::HaI32 stride) { dgVector var (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f)); dgVector cov (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f)); dgVector centre (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f)); dgVector massCenter (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f)); hacd::HaF64 totalArea = hacd::HaF32 (0.0f); const hacd::HaF32* const ptr = vertex; for (hacd::HaI32 i = 0; i < indexCount; i += 3) { hacd::HaI32 index = faceIndex[i] * stride; dgVector p0 (&ptr[index]); p0 = p0.CompProduct (scaleVector); index = faceIndex[i + 1] * stride;; dgVector p1 (&ptr[index]); p1 = p1.CompProduct (scaleVector); index = faceIndex[i + 2] * stride;; dgVector p2 (&ptr[index]); p2 = p2.CompProduct (scaleVector); dgVector normal ((p1 - p0) * (p2 - p0)); hacd::HaF64 area = hacd::HaF32 (0.5f) * sqrt (normal % normal); centre = p0 + p1 + p2; centre = centre.Scale (hacd::HaF32 (1.0f / 3.0f)); // Inertia of each point in the triangle hacd::HaF64 Ixx = p0.m_x * p0.m_x + p1.m_x * p1.m_x + p2.m_x * p2.m_x; hacd::HaF64 Iyy = p0.m_y * p0.m_y + p1.m_y * p1.m_y + p2.m_y * p2.m_y; hacd::HaF64 Izz = p0.m_z * p0.m_z + p1.m_z * p1.m_z + p2.m_z * p2.m_z; hacd::HaF64 Ixy = p0.m_x * p0.m_y + p1.m_x * p1.m_y + p2.m_x * p2.m_y; hacd::HaF64 Iyz = p0.m_y * p0.m_z + p1.m_y * p1.m_z + p2.m_y * p2.m_z; hacd::HaF64 Ixz = p0.m_x * p0.m_z + p1.m_x * p1.m_z + p2.m_x * p2.m_z; if (area > dgEPSILON * 10.0) { hacd::HaF64 K = area / hacd::HaF64 (12.0); //Coriolis theorem for Inertia of a triangle in an arbitrary orientation Ixx = K * (Ixx + 9.0 * centre.m_x * centre.m_x); Iyy = K * (Iyy + 9.0 * centre.m_y * centre.m_y); Izz = K * (Izz + 9.0 * centre.m_z * centre.m_z); Ixy = K * (Ixy + 9.0 * centre.m_x * centre.m_y); Ixz = K * (Ixz + 9.0 * centre.m_x * centre.m_z); Iyz = K * (Iyz + 9.0 * centre.m_y * centre.m_z); centre = centre.Scale ((hacd::HaF32)area); } totalArea += area; massCenter += centre; var += dgVector ((hacd::HaF32)Ixx, (hacd::HaF32)Iyy, (hacd::HaF32)Izz, hacd::HaF32 (0.0f)); cov += dgVector ((hacd::HaF32)Ixy, (hacd::HaF32)Ixz, (hacd::HaF32)Iyz, hacd::HaF32 (0.0f)); } if (totalArea > dgEPSILON * 10.0) { hacd::HaF64 K = hacd::HaF64 (1.0) / totalArea; var = var.Scale ((hacd::HaF32)K); cov = cov.Scale ((hacd::HaF32)K); massCenter = massCenter.Scale ((hacd::HaF32)K); } hacd::HaF64 Ixx = var.m_x - massCenter.m_x * massCenter.m_x; hacd::HaF64 Iyy = var.m_y - massCenter.m_y * massCenter.m_y; hacd::HaF64 Izz = var.m_z - massCenter.m_z * massCenter.m_z; hacd::HaF64 Ixy = cov.m_x - massCenter.m_x * massCenter.m_y; hacd::HaF64 Ixz = cov.m_y - massCenter.m_x * massCenter.m_z; hacd::HaF64 Iyz = cov.m_z - massCenter.m_y * massCenter.m_z; sphere.m_front = dgVector ((hacd::HaF32)Ixx, (hacd::HaF32)Ixy, (hacd::HaF32)Ixz, hacd::HaF32 (0.0f)); sphere.m_up = dgVector ((hacd::HaF32)Ixy, (hacd::HaF32)Iyy, (hacd::HaF32)Iyz, hacd::HaF32 (0.0f)); sphere.m_right = dgVector ((hacd::HaF32)Ixz, (hacd::HaF32)Iyz, (hacd::HaF32)Izz, hacd::HaF32 (0.0f)); sphere.EigenVectors(eigenValues); }
void GfxMesh::addCube(float _size, Vec3f _pos, Vec4f _color, bool _front, bool _back, bool _right, bool _left, bool _top, bool _bottom) { _size /= 2.0f; Vec3f p0(_pos.x - _size, _pos.y - _size, _pos.z + _size); Vec3f p1(_pos.x + _size, _pos.y - _size, _pos.z + _size); Vec3f p2(_pos.x + _size, _pos.y + _size, _pos.z + _size); Vec3f p3(_pos.x - _size, _pos.y + _size, _pos.z + _size); Vec3f p4(_pos.x + _size, _pos.y - _size, _pos.z - _size); Vec3f p5(_pos.x - _size, _pos.y - _size, _pos.z - _size); Vec3f p6(_pos.x - _size, _pos.y + _size, _pos.z - _size); Vec3f p7(_pos.x + _size, _pos.y + _size, _pos.z - _size); Vec3f n; unsigned int v0, v1, v2, v3, v4, v5, v6, v7; // Front if (_front) { n = Vec3f(0.0f, 0.0f, 1.0f); v0 = addVertex(p0, n, _color); v1 = addVertex(p1, n, _color); v2 = addVertex(p2, n, _color); v3 = addVertex(p3, n, _color); addTriangle(v0, v1, v2); addTriangle(v0, v2, v3); } // Back if (_back) { n = Vec3f(0.0f, 0.0f, -1.0f); v4 = addVertex(p4, n, _color); v5 = addVertex(p5, n, _color); v6 = addVertex(p6, n, _color); v7 = addVertex(p7, n, _color); addTriangle(v4, v5, v6); addTriangle(v4, v6, v7); } // Right if (_right) { n = Vec3f(1.0f, 0.0f, 0.0f); v1 = addVertex(p1, n, _color); v4 = addVertex(p4, n, _color); v7 = addVertex(p7, n, _color); v2 = addVertex(p2, n, _color); addTriangle(v1, v4, v7); addTriangle(v1, v7, v2); } // Left if (_left) { n = Vec3f(-1.0f, 0.0f, 0.0f); v5 = addVertex(p5, n, _color); v0 = addVertex(p0, n, _color); v3 = addVertex(p3, n, _color); v6 = addVertex(p6, n, _color); addTriangle(v5, v0, v3); addTriangle(v5, v3, v6); } // Top if (_top) { n = Vec3f(0.0f, 1.0f, 0.0f); v3 = addVertex(p3, n, _color); v2 = addVertex(p2, n, _color); v7 = addVertex(p7, n, _color); v6 = addVertex(p6, n, _color); addTriangle(v3, v2, v7); addTriangle(v3, v7, v6); } // Bottom if (_bottom) { n = Vec3f(0.0f, -1.0f, 0.0f); v5 = addVertex(p5, n, _color); v4 = addVertex(p4, n, _color); v1 = addVertex(p1, n, _color); v0 = addVertex(p0, n, _color); addTriangle(v5, v4, v1); addTriangle(v5, v1, v0); } }
void CreateHeightFieldMesh (NewtonCollision* collision, Entity* ent) { int width; int height; dFloat hScale; dFloat vScale; unsigned short* elevations; NewtonCollisionInfoRecord collisionInfo; // keep the compiler happy memset (&collisionInfo, 0, sizeof (NewtonCollisionInfoRecord)); NewtonCollisionGetInfo (collision, &collisionInfo); // get the info from the collision mesh and create a visual mesh width = collisionInfo.m_heightField.m_width; height = collisionInfo.m_heightField.m_height; elevations = collisionInfo.m_heightField.m_elevation; vScale = collisionInfo.m_heightField.m_verticalScale; hScale = collisionInfo.m_heightField.m_horizonalScale; // allocate space to store vertex data ent->m_vertexCount = width * height; ent->m_vertex = (dFloat*) malloc (3 * width * height * sizeof (dFloat)); ent->m_normal = (dFloat*) malloc (3 * width * height * sizeof (dFloat)); ent->m_uv = (dFloat*) malloc (2 * width * height * sizeof (dFloat)); // scan the height field and convert every cell into two triangles for (int z = 0; z < height; z ++) { int z0; int z1; z0 = ((z - 1) < 0) ? 0 : z - 1; z1 = ((z + 1) > (height - 1)) ? height - 1 : z + 1 ; for (int x = 0; x < width; x ++) { int x0; int x1; x0 = ((x - 1) < 0) ? 0 : x - 1; x1 = ((x + 1) > (width - 1)) ? width - 1 : x + 1 ; dVector p0 (hScale * x0, elevations[z * width + x1] * vScale, hScale * z); dVector p1 (hScale * x1, elevations[z * width + x0] * vScale, hScale * z); dVector x10 (p1 - p0); dVector q0 (hScale * x, elevations[z0 * width + x] * vScale, hScale * z0); dVector q1 (hScale * x, elevations[z1 * width + x] * vScale, hScale * z1); dVector z10 (q1 - q0); dVector normal (z10 * x10); normal = normal.Scale (dSqrt (1.0f / (normal % normal))); dVector point (hScale * x, elevations[z * width + x] * vScale, hScale * z); ent->m_vertex[(z * width + x) * 3 + 0] = point.m_x; ent->m_vertex[(z * width + x) * 3 + 1] = point.m_y; ent->m_vertex[(z * width + x) * 3 + 2] = point.m_z; ent->m_normal[(z * width + x) * 3 + 0] = normal.m_x; ent->m_normal[(z * width + x) * 3 + 1] = normal.m_y; ent->m_normal[(z * width + x) * 3 + 2] = normal.m_z; ent->m_uv[(z * width + x) * 2 + 0] = x * TEXTURE_SCALE; ent->m_uv[(z * width + x) * 2 + 1] = z * TEXTURE_SCALE; } } // since the bitmap sample is 256 x 256, i fix into a single 16 bit index vertex array with ent->m_subMeshCount = 1; ent->m_subMeshes = (Entity::SubMesh*) malloc (sizeof (Entity::SubMesh)); // allocate space to the index list ent->m_subMeshes[0].m_textureHandle = LoadTexture ("grassAndDirt.tga"); ent->m_subMeshes[0].m_indexCount = (width - 1) * (height - 1) * 6; ent->m_subMeshes[0].m_indexArray = (unsigned short*) malloc (ent->m_subMeshes[0].m_indexCount * sizeof (unsigned short)); // now following the grid pattern and create and index list int index; int vertexIndex; index = 0; vertexIndex = 0; for (int z = 0; z < height - 1; z ++) { vertexIndex = z * width; for (int x = 0; x < width - 1; x ++) { ent->m_subMeshes[0].m_indexArray[index + 0] = GLushort (vertexIndex); ent->m_subMeshes[0].m_indexArray[index + 1] = GLushort (vertexIndex + width); ent->m_subMeshes[0].m_indexArray[index + 2] = GLushort (vertexIndex + 1); index += 3; ent->m_subMeshes[0].m_indexArray[index + 0] = GLushort (vertexIndex + 1); ent->m_subMeshes[0].m_indexArray[index + 1] = GLushort (vertexIndex + width); ent->m_subMeshes[0].m_indexArray[index + 2] = GLushort (vertexIndex + width + 1); index += 3; vertexIndex ++; } } // Optimize the mesh for hardware rendering if possible ent->OptimizeMesh(); /* dVector boxP0; dVector boxP1; // get the position of the aabb of this geometry dMatrix matrix (ent->m_curRotation, ent->m_curPosition); NewtonCollisionCalculateAABB (collision, &matrix[0][0], &boxP0.m_x, &boxP1.m_x); // place the origin of the visual mesh at the center of the height field matrix.m_posit = (boxP0 + boxP1).Scale (-0.5f); matrix.m_posit.m_w = 1.0f; ent->m_curPosition = matrix.m_posit; ent->m_prevPosition = matrix.m_posit; // create the level rigid body body = NewtonCreateBody(world, collision); // release the collision tree (this way the application does not have to do book keeping of Newton objects NewtonReleaseCollision (world, collision); // save the pointer to the graphic object with the body. NewtonBodySetUserData (body, ent); // set the global position of this body NewtonBodySetMatrix (body, &matrix[0][0]); // set the destructor for this object // NewtonBodySetDestructorCallback (body, Destructor); // get the position of the aabb of this geometry NewtonCollisionCalculateAABB (collision, &matrix[0][0], &boxP0.m_x, &boxP1.m_x); // add some extra padding the world size boxP0.m_x -= 10.0f; boxP0.m_y -= 10.0f; boxP0.m_z -= 10.0f; boxP1.m_x += 10.0f; boxP1.m_y += 400.0f; boxP1.m_z += 10.0f; // set the world size NewtonSetWorldSize (world, &boxP0.m_x, &boxP1.m_x); return body; */ }