SimdVector3 GetRayTo(int x,int y) { float top = 1.f; float bottom = -1.f; float nearPlane = 1.f; float tanFov = (top-bottom)*0.5f / nearPlane; float fov = 2.0 * atanf (tanFov); SimdVector3 rayFrom(eye[0],eye[1],eye[2]); SimdVector3 rayForward = -rayFrom; rayForward.normalize(); float farPlane = 600.f; rayForward*= farPlane; SimdVector3 rightOffset; SimdVector3 vertical(0.f,1.f,0.f); SimdVector3 hor; hor = rayForward.cross(vertical); hor.normalize(); vertical = hor.cross(rayForward); vertical.normalize(); float tanfov = tanf(0.5f*fov); hor *= 2.f * farPlane * tanfov; vertical *= 2.f * farPlane * tanfov; SimdVector3 rayToCenter = rayFrom + rayForward; SimdVector3 dHor = hor * 1.f/float(glutScreenWidth); SimdVector3 dVert = vertical * 1.f/float(glutScreenHeight); SimdVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical; rayTo += x * dHor; rayTo -= y * dVert; return rayTo; }
void Raytracer::displayCallback() { updateCamera(); for (int i=0;i<numObjects;i++) { transforms[i].setIdentity(); SimdVector3 pos(-3.5f+i*2.5f,0.f,0.f); transforms[i].setOrigin( pos ); SimdQuaternion orn; if (i < 2) { orn.setEuler(yaw,pitch,roll); transforms[i].setRotation(orn); } } myMink.SetTransformA(SimdTransform(transforms[0].getRotation())); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glDisable(GL_LIGHTING); if (once) { glGenTextures(1, &glTextureId); glBindTexture(GL_TEXTURE_2D,glTextureId ); once = 0; glPixelStorei(GL_UNPACK_ALIGNMENT, 1); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); } glDisable(GL_TEXTURE_2D); glDisable(GL_BLEND); #define RAYTRACER #ifdef RAYTRACER SimdVector4 rgba(1.f,0.f,0.f,0.5f); float top = 1.f; float bottom = -1.f; float nearPlane = 1.f; float tanFov = (top-bottom)*0.5f / nearPlane; float fov = 2.0 * atanf (tanFov); SimdVector3 rayFrom = getCameraPosition(); SimdVector3 rayForward = getCameraTargetPosition()-getCameraPosition(); rayForward.normalize(); float farPlane = 600.f; rayForward*= farPlane; SimdVector3 rightOffset; SimdVector3 vertical(0.f,1.f,0.f); SimdVector3 hor; hor = rayForward.cross(vertical); hor.normalize(); vertical = hor.cross(rayForward); vertical.normalize(); float tanfov = tanf(0.5f*fov); hor *= 2.f * farPlane * tanfov; vertical *= 2.f * farPlane * tanfov; SimdVector3 rayToCenter = rayFrom + rayForward; SimdVector3 dHor = hor * 1.f/float(screenWidth); SimdVector3 dVert = vertical * 1.f/float(screenHeight); SimdTransform rayFromTrans; rayFromTrans.setIdentity(); rayFromTrans.setOrigin(rayFrom); SimdTransform rayFromLocal; SimdTransform rayToLocal; SphereShape pointShape(0.0f); ///clear texture for (int x=0;x<screenWidth;x++) { for (int y=0;y<screenHeight;y++) { SimdVector4 rgba(0.f,0.f,0.f,0.f); raytracePicture->SetPixel(x,y,rgba); } } ConvexCast::CastResult rayResult; SimdTransform rayToTrans; rayToTrans.setIdentity(); SimdVector3 rayTo; for (int x=0;x<screenWidth;x++) { for (int y=0;y<screenHeight;y++) { rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical; rayTo += x * dHor; rayTo -= y * dVert; rayToTrans.setOrigin(rayTo); for (int s=0;s<numObjects;s++) { // rayFromLocal = transforms[s].inverse()* rayFromTrans; // rayToLocal = transforms[s].inverse()* rayToTrans; //choose the continuous collision detection method SubsimplexConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver); //GjkConvexCast convexCaster(&pointShape,shapePtr[0],&simplexSolver); //ContinuousConvexCollision convexCaster(&pointShape,shapePtr[0],&simplexSolver,0); // BU_Simplex1to4 ptShape(SimdVector3(0,0,0));//algebraic needs features, doesnt use 'supporting vertex' // BU_CollisionPair convexCaster(&ptShape,shapePtr[0]); //reset previous result rayResult.m_fraction = 1.f; if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,transforms[s],transforms[s],rayResult)) { //float fog = 1.f - 0.1f * rayResult.m_fraction; rayResult.m_normal.normalize(); SimdVector3 worldNormal; worldNormal = transforms[s].getBasis() *rayResult.m_normal; float light = worldNormal.dot(SimdVector3(0.4f,-1.f,-0.4f)); if (light < 0.2f) light = 0.2f; if (light > 1.f) light = 1.f; rgba = SimdVector4(light,light,light,1.f); raytracePicture->SetPixel(x,y,rgba); } else { //clear is already done //rgba = SimdVector4(0.f,0.f,0.f,0.f); //raytracePicture->SetPixel(x,y,rgba); } } } } #define TEST_PRINTF #ifdef TEST_PRINTF extern BMF_FontData BMF_font_helv10; raytracePicture->Printf("CCD RAYTRACER",&BMF_font_helv10); char buffer[256]; sprintf(buffer,"%d RAYS / Frame",screenWidth*screenHeight*numObjects); raytracePicture->Printf(buffer,&BMF_font_helv10,0,10); #endif //TEST_PRINTF glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); glFrustum(-1.0,1.0,-1.0,1.0,3,2020.0); glMatrixMode(GL_MODELVIEW); glPushMatrix(); glLoadIdentity(); // Reset The Modelview Matrix glTranslatef(0.0f,0.0f,-3.0f); // Move Into The Screen 5 Units glEnable(GL_TEXTURE_2D); glBindTexture(GL_TEXTURE_2D,glTextureId ); const unsigned char *ptr = raytracePicture->GetBuffer(); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, raytracePicture->GetWidth(),raytracePicture->GetHeight(), 0, GL_RGBA, GL_UNSIGNED_BYTE, ptr); glEnable (GL_BLEND); glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glColor4f (1,1,1,1); // alpha=0.5=half visible glBegin(GL_QUADS); glTexCoord2f(0.0f, 0.0f); glVertex2f(-1,1); glTexCoord2f(1.0f, 0.0f); glVertex2f(1,1); glTexCoord2f(1.0f, 1.0f); glVertex2f(1,-1); glTexCoord2f(0.0f, 1.0f); glVertex2f(-1,-1); glEnd(); glMatrixMode(GL_MODELVIEW); glPopMatrix(); glMatrixMode(GL_PROJECTION); glPopMatrix(); glMatrixMode(GL_MODELVIEW); #endif //RAYRACER glDisable(GL_TEXTURE_2D); glDisable(GL_DEPTH_TEST); GL_ShapeDrawer::DrawCoordSystem(); glPushMatrix(); /* /// normal opengl rendering float m[16]; int i; for (i=0;i<numObjects;i++) { transA.getOpenGLMatrix( m ); /// draw the simplex GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1)); /// calculate closest point from simplex to the origin, and draw this vector simplex.CalcClosest(m); } */ glPopMatrix(); pitch += 0.005f; yaw += 0.01f; glFlush(); glutSwapBuffers(); }
bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSolver, ConvexShape* convexA,ConvexShape* convexB, const SimdTransform& transformA,const SimdTransform& transformB, SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb) { int num_verts = simplexSolver.getSimplex(pBuf, qBuf, yBuf); switch (num_verts) { case 1: // Touching contact. Yes, we have a collision, // but no penetration. return false; case 2: { // We have a line segment inside the Minkowski sum containing the // origin. Blow it up by adding three additional support points. SimdVector3 dir = (yBuf[1] - yBuf[0]).normalized(); int axis = dir.furthestAxis(); static SimdScalar sin_60 = 0.8660254037f;//84438646763723170752941.22474487f;//13915890490986420373529;// SimdQuaternion rot(dir[0] * sin_60, dir[1] * sin_60, dir[2] * sin_60, SimdScalar(0.5)); SimdMatrix3x3 rot_mat(rot); SimdVector3 aux1 = dir.cross(SimdVector3(axis == 0, axis == 1, axis == 2)); SimdVector3 aux2 = rot_mat * aux1; SimdVector3 aux3 = rot_mat * aux2; pBuf[2] = transformA(convexA->LocalGetSupportingVertex(aux1*transformA.getBasis())); qBuf[2] = transformB(convexB->LocalGetSupportingVertex((-aux1)*transformB.getBasis())); yBuf[2] = pBuf[2] - qBuf[2]; pBuf[3] = transformA(convexA->LocalGetSupportingVertex(aux2*transformA.getBasis())); qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-aux2)*transformB.getBasis())); yBuf[3] = pBuf[3] - qBuf[3]; pBuf[4] = transformA(convexA->LocalGetSupportingVertex(aux3*transformA.getBasis())); qBuf[4] = transformB(convexB->LocalGetSupportingVertex((-aux3)*transformB.getBasis())); yBuf[4] = pBuf[4] - qBuf[4]; if (originInTetrahedron(yBuf[0], yBuf[2], yBuf[3], yBuf[4])) { pBuf[1] = pBuf[4]; qBuf[1] = qBuf[4]; yBuf[1] = yBuf[4]; } else if (originInTetrahedron(yBuf[1], yBuf[2], yBuf[3], yBuf[4])) { pBuf[0] = pBuf[4]; qBuf[0] = qBuf[4]; yBuf[0] = yBuf[4]; } else { // Origin not in initial polytope return false; } num_verts = 4; break; } case 3: { // We have a triangle inside the Minkowski sum containing // the origin. First blow it up. SimdVector3 v1 = yBuf[1] - yBuf[0]; SimdVector3 v2 = yBuf[2] - yBuf[0]; SimdVector3 vv = v1.cross(v2); pBuf[3] = transformA(convexA->LocalGetSupportingVertex(vv*transformA.getBasis())); qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-vv)*transformB.getBasis())); yBuf[3] = pBuf[3] - qBuf[3]; pBuf[4] = transformA(convexA->LocalGetSupportingVertex((-vv)*transformA.getBasis())); qBuf[4] = transformB(convexB->LocalGetSupportingVertex(vv*transformB.getBasis())); yBuf[4] = pBuf[4] - qBuf[4]; if (originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[4])) { pBuf[3] = pBuf[4]; qBuf[3] = qBuf[4]; yBuf[3] = yBuf[4]; } else if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3])) { // Origin not in initial polytope return false; } num_verts = 4; break; } } // We have a tetrahedron inside the Minkowski sum containing // the origin (if GJK did it's job right ;-) if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3])) { // assert(false); return false; } num_facets = 0; freeFacet = 0; ReplaceMeFacet *f0 = addFacet(0, 1, 2, SimdScalar(0.0), SIMD_INFINITY); ReplaceMeFacet *f1 = addFacet(0, 3, 1, SimdScalar(0.0), SIMD_INFINITY); ReplaceMeFacet *f2 = addFacet(0, 2, 3, SimdScalar(0.0), SIMD_INFINITY); ReplaceMeFacet *f3 = addFacet(1, 3, 2, SimdScalar(0.0), SIMD_INFINITY); if (!f0 || f0->getDist2() == SimdScalar(0.0) || !f1 || f1->getDist2() == SimdScalar(0.0) || !f2 || f2->getDist2() == SimdScalar(0.0) || !f3 || f3->getDist2() == SimdScalar(0.0)) { return false; } f0->link(0, f1, 2); f0->link(1, f3, 2); f0->link(2, f2, 0); f1->link(0, f2, 2); f1->link(1, f3, 0); f2->link(1, f3, 1); if (num_facets == 0) { return false; } // at least one facet on the heap. ReplaceMeEdgeBuffer edgeBuffer(20); ReplaceMeFacet *facet = 0; SimdScalar upper_bound2 = SIMD_INFINITY; do { facet = facetHeap[0]; std::pop_heap(&facetHeap[0], &facetHeap[num_facets], myFacetComp); --num_facets; if (!facet->isObsolete()) { assert(facet->getDist2() > SimdScalar(0.0)); if (num_verts == MaxSupportPoints) { #ifdef DEBUG std::cout << "Ouch, no convergence!!!" << std::endl; #endif ASSERT_MESSAGE(false,"Error: pendepth calc failed"); break; } pBuf[num_verts] = transformA(convexA->LocalGetSupportingVertex((facet->getClosest())*transformA.getBasis())); qBuf[num_verts] = transformB(convexB->LocalGetSupportingVertex((-facet->getClosest())*transformB.getBasis())); yBuf[num_verts] = pBuf[num_verts] - qBuf[num_verts]; int index = num_verts++; SimdScalar far_dist2 = yBuf[index].dot(facet->getClosest()); // Make sure the support mapping is OK. //assert(far_dist2 > SimdScalar(0.0)); // // this is to avoid problems with implicit-sphere-touching contact // if (far_dist2 < SimdScalar(0.0)) { return false; } GEN_set_min(upper_bound2, (far_dist2 * far_dist2) / facet->getDist2()); if (upper_bound2 <= ReplaceMeAccuracy::depth_tolerance * facet->getDist2() #define CHECK_NEW_SUPPORT #ifdef CHECK_NEW_SUPPORT || yBuf[index] == yBuf[(*facet)[0]] || yBuf[index] == yBuf[(*facet)[1]] || yBuf[index] == yBuf[(*facet)[2]] #endif ) { break; } // Compute the silhouette cast by the new vertex // Note that the new vertex is on the positive side // of the current facet, so the current facet is will // not be in the convex hull. Start local search // from this facet. facet->silhouette(yBuf[index], edgeBuffer); if (edgeBuffer.empty()) { return false; } ReplaceMeEdgeBuffer::const_iterator it = edgeBuffer.begin(); ReplaceMeFacet *firstFacet = addFacet((*it).getTarget(), (*it).getSource(), index, facet->getDist2(), upper_bound2); if (!firstFacet) { break; } firstFacet->link(0, (*it).getFacet(), (*it).getIndex()); ReplaceMeFacet *lastFacet = firstFacet; ++it; for (; it != edgeBuffer.end(); ++it) { ReplaceMeFacet *newFacet = addFacet((*it).getTarget(), (*it).getSource(), index, facet->getDist2(), upper_bound2); if (!newFacet) { break; } if (!newFacet->link(0, (*it).getFacet(), (*it).getIndex())) { break; } if (!newFacet->link(2, lastFacet, 1)) { break; } lastFacet = newFacet; } if (it != edgeBuffer.end()) { break; } firstFacet->link(2, lastFacet, 1); } } while (num_facets > 0 && facetHeap[0]->getDist2() <= upper_bound2); #ifdef DEBUG std::cout << "#facets left = " << num_facets << std::endl; #endif v = facet->getClosest(); pa = facet->getClosestPoint(pBuf); pb = facet->getClosestPoint(qBuf); return true; }
bool Epa::Initialize( SimplexSolverInterface& simplexSolver ) { // Run GJK on the enlarged shapes to obtain a simplex of the enlarged CSO SimdVector3 v( 1, 0, 0 ); SimdScalar squaredDistance = SIMD_INFINITY; SimdScalar delta = 0.f; simplexSolver.reset(); int nbIterations = 0; while ( true ) { EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Warning : v has zero magnitude!" ); SimdVector3 seperatingAxisInA = -v * m_transformA.getBasis(); SimdVector3 seperatingAxisInB = v * m_transformB.getBasis(); SimdVector3 pInA = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA ); SimdVector3 qInB = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB ); SimdPoint3 pWorld = m_transformA( pInA ); SimdPoint3 qWorld = m_transformB( qInB ); SimdVector3 w = pWorld - qWorld; delta = v.dot( w ); EPA_DEBUG_ASSERT( ( delta <= 0 ) ,"Shapes are disjoint, EPA should have never been called!" ); if ( delta > 0.f ) return false; EPA_DEBUG_ASSERT( !simplexSolver.inSimplex( w ) ,"Shapes are disjoint, EPA should have never been called!" ); if (simplexSolver.inSimplex( w )) return false; // Add support point to simplex simplexSolver.addVertex( w, pWorld, qWorld ); bool closestOk = simplexSolver.closest( v ); EPA_DEBUG_ASSERT( closestOk ,"Shapes are disjoint, EPA should have never been called!" ); if (!closestOk) return false; SimdScalar prevVSqrd = squaredDistance; squaredDistance = v.length2(); // Is v converging to v(A-B) ? EPA_DEBUG_ASSERT( ( ( prevVSqrd - squaredDistance ) > SIMD_EPSILON * prevVSqrd ) , "Shapes are disjoint, EPA should have never been called!" ); if (( ( prevVSqrd - squaredDistance ) <= SIMD_EPSILON * prevVSqrd )) return false; if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) ) { break; } ++nbIterations; } SimdPoint3 simplexPoints[ 5 ]; SimdPoint3 wSupportPointsOnA[ 5 ]; SimdPoint3 wSupportPointsOnB[ 5 ]; int nbSimplexPoints = simplexSolver.getSimplex( wSupportPointsOnA, wSupportPointsOnB, simplexPoints ); // nbSimplexPoints can't be one because cases where the origin is on the boundary are handled // by hybrid penetration depth EPA_DEBUG_ASSERT( ( ( nbSimplexPoints > 1 ) ,( nbSimplexPoints <= 4 ) ) , "Hybrid Penetration Depth algorithm failed!" ); int nbPolyhedronPoints = nbSimplexPoints; #ifndef EPA_POLYHEDRON_USE_PLANES int initTetraIndices[ 4 ] = { 0, 1, 2, 3 }; #endif // Prepare initial polyhedron to start EPA from if ( nbSimplexPoints == 1 ) { return false; } else if ( nbSimplexPoints == 2 ) { // We have a line segment inside the CSO that contains the origin // Create an hexahedron ( two tetrahedron glued together ) by adding 3 new points SimdVector3 d = simplexPoints[ 0 ] - simplexPoints[ 1 ]; d.normalize(); SimdVector3 v1; SimdVector3 v2; SimdVector3 v3; SimdVector3 e1; SimdScalar absx = abs( d.getX() ); SimdScalar absy = abs( d.getY() ); SimdScalar absz = abs( d.getZ() ); if ( absx < absy ) { if ( absx < absz ) { e1.setX( 1 ); } else { e1.setZ( 1 ); } } else { if ( absy < absz ) { e1.setY( 1 ); } else { e1.setZ( 1 ); } } v1 = d.cross( e1 ); v1.normalize(); v2 = v1.rotate( d, 120 * SIMD_RADS_PER_DEG ); v3 = v2.rotate( d, 120 * SIMD_RADS_PER_DEG ); nbPolyhedronPoints = 5; SimdVector3 seperatingAxisInA = v1 * m_transformA.getBasis(); SimdVector3 seperatingAxisInB = -v1 * m_transformB.getBasis(); SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA ); SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB ); SimdPoint3 pWorld = m_transformA( p ); SimdPoint3 qWorld = m_transformB( q ); wSupportPointsOnA[ 2 ] = pWorld; wSupportPointsOnB[ 2 ] = qWorld; simplexPoints[ 2 ] = wSupportPointsOnA[ 2 ] - wSupportPointsOnB[ 2 ]; seperatingAxisInA = v2 * m_transformA.getBasis(); seperatingAxisInB = -v2 * m_transformB.getBasis(); p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA ); q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB ); pWorld = m_transformA( p ); qWorld = m_transformB( q ); wSupportPointsOnA[ 3 ] = pWorld; wSupportPointsOnB[ 3 ] = qWorld; simplexPoints[ 3 ] = wSupportPointsOnA[ 3 ] - wSupportPointsOnB[ 3 ]; seperatingAxisInA = v3 * m_transformA.getBasis(); seperatingAxisInB = -v3 * m_transformB.getBasis(); p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA ); q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB ); pWorld = m_transformA( p ); qWorld = m_transformB( q ); wSupportPointsOnA[ 4 ] = pWorld; wSupportPointsOnB[ 4 ] = qWorld; simplexPoints[ 4 ] = wSupportPointsOnA[ 4 ] - wSupportPointsOnB[ 4 ]; #ifndef EPA_POLYHEDRON_USE_PLANES if ( TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 2 ], simplexPoints[ 3 ], simplexPoints[ 4 ] ) ) { initTetraIndices[ 1 ] = 2; initTetraIndices[ 2 ] = 3; initTetraIndices[ 3 ] = 4; } else { if ( TetrahedronContainsOrigin( simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 3 ], simplexPoints[ 4 ] ) ) { initTetraIndices[ 0 ] = 1; initTetraIndices[ 1 ] = 2; initTetraIndices[ 2 ] = 3; initTetraIndices[ 3 ] = 4; } else { // No tetrahedron contains the origin assert( false && "Unable to find an initial tetrahedron that contains the origin!" ); return false; } } #endif } else if ( nbSimplexPoints == 3 ) { // We have a triangle inside the CSO that contains the origin // Create an hexahedron ( two tetrahedron glued together ) by adding 2 new points SimdVector3 v0 = simplexPoints[ 2 ] - simplexPoints[ 0 ]; SimdVector3 v1 = simplexPoints[ 1 ] - simplexPoints[ 0 ]; SimdVector3 triangleNormal = v0.cross( v1 ); triangleNormal.normalize(); nbPolyhedronPoints = 5; SimdVector3 seperatingAxisInA = triangleNormal * m_transformA.getBasis(); SimdVector3 seperatingAxisInB = -triangleNormal * m_transformB.getBasis(); SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA ); SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB ); SimdPoint3 pWorld = m_transformA( p ); SimdPoint3 qWorld = m_transformB( q ); wSupportPointsOnA[ 3 ] = pWorld; wSupportPointsOnB[ 3 ] = qWorld; simplexPoints[ 3 ] = wSupportPointsOnA[ 3 ] - wSupportPointsOnB[ 3 ]; #ifndef EPA_POLYHEDRON_USE_PLANES // We place this check here because if the tetrahedron contains the origin // there is no need to sample another support point if ( !TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 3 ] ) ) { #endif seperatingAxisInA = -triangleNormal * m_transformA.getBasis(); seperatingAxisInB = triangleNormal * m_transformB.getBasis(); p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA ); q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB ); pWorld = m_transformA( p ); qWorld = m_transformB( q ); wSupportPointsOnA[ 4 ] = pWorld; wSupportPointsOnB[ 4 ] = qWorld; simplexPoints[ 4 ] = wSupportPointsOnA[ 4 ] - wSupportPointsOnB[ 4 ]; #ifndef EPA_POLYHEDRON_USE_PLANES if ( TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 4 ] ) ) { initTetraIndices[ 3 ] = 4; } else { // No tetrahedron contains the origin assert( false && "Unable to find an initial tetrahedron that contains the origin!" ); return false; } } #endif } #ifdef _DEBUG else if ( nbSimplexPoints == 4 ) { EPA_DEBUG_ASSERT( TetrahedronContainsOrigin( simplexPoints ) ,"Initial tetrahedron does not contain the origin!" ); } #endif #ifndef EPA_POLYHEDRON_USE_PLANES SimdPoint3 wTetraPoints[ 4 ] = { simplexPoints[ initTetraIndices[ 0 ] ], simplexPoints[ initTetraIndices[ 1 ] ], simplexPoints[ initTetraIndices[ 2 ] ], simplexPoints[ initTetraIndices[ 3 ] ] }; SimdPoint3 wTetraSupportPointsOnA[ 4 ] = { wSupportPointsOnA[ initTetraIndices[ 0 ] ], wSupportPointsOnA[ initTetraIndices[ 1 ] ], wSupportPointsOnA[ initTetraIndices[ 2 ] ], wSupportPointsOnA[ initTetraIndices[ 3 ] ] }; SimdPoint3 wTetraSupportPointsOnB[ 4 ] = { wSupportPointsOnB[ initTetraIndices[ 0 ] ], wSupportPointsOnB[ initTetraIndices[ 1 ] ], wSupportPointsOnB[ initTetraIndices[ 2 ] ], wSupportPointsOnB[ initTetraIndices[ 3 ] ] }; #endif #ifdef EPA_POLYHEDRON_USE_PLANES if ( !m_polyhedron.Create( simplexPoints, wSupportPointsOnA, wSupportPointsOnB, nbPolyhedronPoints ) ) #else if ( !m_polyhedron.Create( wTetraPoints, wTetraSupportPointsOnA, wTetraSupportPointsOnB, 4 ) ) #endif { // Failed to create initial polyhedron EPA_DEBUG_ASSERT( false ,"Failed to create initial polyhedron!" ); return false; } // Add initial faces to priority queue #ifdef _DEBUG //m_polyhedron._dbgSaveToFile( "epa_start.dbg" ); #endif std::list< EpaFace* >& faces = m_polyhedron.GetFaces(); std::list< EpaFace* >::iterator facesItr( faces.begin() ); while ( facesItr != faces.end() ) { EpaFace* pFace = *facesItr; if ( !pFace->m_deleted ) { //#ifdef EPA_POLYHEDRON_USE_PLANES // if ( pFace->m_planeDistance >= 0 ) // { // m_polyhedron._dbgSaveToFile( "epa_start.dbg" ); // assert( false && "Face's plane distance equal or greater than 0!" ); // } //#endif if ( pFace->IsAffinelyDependent() ) { EPA_DEBUG_ASSERT( false ,"One initial face is affinely dependent!" ); return false; } if ( pFace->m_vSqrd <= 0 ) { EPA_DEBUG_ASSERT( false ,"Face containing the origin!" ); return false; } if ( pFace->IsClosestPointInternal() ) { m_faceEntries.push_back( pFace ); std::push_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries ); } } ++facesItr; } #ifdef _DEBUG //m_polyhedron._dbgSaveToFile( "epa_start.dbg" ); #endif EPA_DEBUG_ASSERT( !m_faceEntries.empty() ,"No faces added to heap!" ); return true; }