SimdVector3 PolyhedralConvexShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec0)const { int i; SimdVector3 supVec(0,0,0); SimdScalar maxDot(-1e30f); SimdVector3 vec = vec0; SimdScalar lenSqr = vec.length2(); if (lenSqr < 0.0001f) { vec.setValue(1,0,0); } else { float rlen = 1.f / SimdSqrt(lenSqr ); vec *= rlen; } SimdVector3 vtx; SimdScalar newDot; for (i=0;i<GetNumVertices();i++) { GetVertex(i,vtx); newDot = vec.dot(vtx); if (newDot > maxDot) { maxDot = newDot; supVec = vtx; } } return supVec; }
virtual void InternalProcessTriangleIndex(SimdVector3* triangle,int partId,int triangleIndex) { for (int i=0;i<3;i++) { SimdScalar dot = m_supportVecLocal.dot(triangle[i]); if (dot > m_maxDot) { m_maxDot = dot; m_supportVertexLocal = triangle[i]; } } }
void ReplaceMeFacet::silhouette(int index, const SimdVector3& w, ReplaceMeEdgeBuffer& edgeBuffer) { if (!m_obsolete) { if (m_closest.dot(w) < m_dist2) { edgeBuffer.push_back(ReplaceMeEdge(this, index)); } else { m_obsolete = true; // Facet is visible int next = incMod3(index); m_adjFacets[next]->silhouette(m_adjEdges[next], w, edgeBuffer); next = incMod3(next); m_adjFacets[next]->silhouette(m_adjEdges[next], w, edgeBuffer); } } }
bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexSolver, ConvexShape* pConvexA, ConvexShape* pConvexB, const SimdTransform& transformA, const SimdTransform& transformB, SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB, SimdScalar& penDepth, SimdVector3& v ) { SimdScalar squaredDistance = SIMD_INFINITY; SimdScalar delta = 0.f; const SimdScalar margin = pConvexA->GetMargin() + pConvexB->GetMargin(); const SimdScalar marginSqrd = margin * margin; simplexSolver.reset(); int nbIterations = 0; while ( true ) { assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" ); SimdVector3 seperatingAxisInA = -v * transformA.getBasis(); SimdVector3 seperatingAxisInB = v * transformB.getBasis(); SimdVector3 pInA = pConvexA->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA ); SimdVector3 qInB = pConvexB->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB ); SimdPoint3 pWorld = transformA( pInA ); SimdPoint3 qWorld = transformB( qInB ); SimdVector3 w = pWorld - qWorld; delta = v.dot( w ); // potential exit, they don't overlap if ( ( delta > 0 ) && ( ( delta * delta / squaredDistance ) > marginSqrd ) ) { // Convex shapes do not overlap // Returning true means that Hybrid's result is ok and there's no need to run EPA penDepth = 0; return true; } //exit 0: the new point is already in the simplex, or we didn't come any closer if ( ( squaredDistance - delta <= squaredDistance * g_GJKMaxRelErrorSqrd ) || simplexSolver.inSimplex( w ) ) { simplexSolver.compute_points( wWitnessOnA, wWitnessOnB ); assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); SimdScalar vLength = sqrt( squaredDistance ); wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength ); wWitnessOnB += v * ( pConvexB->GetMargin() / vLength ); penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength; // Returning true means that Hybrid's result is ok and there's no need to run EPA return true; } //add current vertex to simplex simplexSolver.addVertex( w, pWorld, qWorld ); //calculate the closest point to the origin (update vector v) if ( !simplexSolver.closest( v ) ) { simplexSolver.compute_points( wWitnessOnA, wWitnessOnB ); assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); SimdScalar vLength = sqrt( squaredDistance ); wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength ); wWitnessOnB += v * ( pConvexB->GetMargin() / vLength ); penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength; // Returning true means that Hybrid's result is ok and there's no need to run EPA return true; } SimdScalar previousSquaredDistance = squaredDistance; squaredDistance = v.length2(); //are we getting any closer ? if ( previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance ) { simplexSolver.backup_closest( v ); squaredDistance = v.length2(); simplexSolver.compute_points( wWitnessOnA, wWitnessOnB ); assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); SimdScalar vLength = sqrt( squaredDistance ); wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength ); wWitnessOnB += v * ( pConvexB->GetMargin() / vLength ); penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength; // Returning true means that Hybrid's result is ok and there's no need to run EPA return true; } if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) ) { // Convex Shapes intersect - we need to run EPA // Returning false means that Hybrid couldn't do anything for us // and that we need to run EPA to calculate the pen depth return false; } ++nbIterations; } }
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(); }
SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB ) { SimdVector3 v; SimdScalar upperBoundSqrd = SIMD_INFINITY; SimdScalar vSqrd = 0; #ifdef _DEBUG SimdScalar prevVSqrd; #endif SimdScalar delta; bool isCloseEnough = false; EpaFace* pEpaFace = NULL; int nbIterations = 0; //int nbMaxIterations = 1000; do { pEpaFace = m_faceEntries.front(); std::pop_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries ); m_faceEntries.pop_back(); if ( !pEpaFace->m_deleted ) { #ifdef _DEBUG prevVSqrd = vSqrd; #endif vSqrd = pEpaFace->m_vSqrd; if ( pEpaFace->m_planeDistance >= 0 ) { v = pEpaFace->m_planeNormal; } else { v = pEpaFace->m_v; } #ifdef _DEBUG //assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" ); EPA_DEBUG_ASSERT( ( vSqrd >= prevVSqrd ) ,"vSqrd decreased!" ); #endif //_DEBUG EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Zero vector not allowed!" ); SimdVector3 seperatingAxisInA = v * m_transformA.getBasis(); SimdVector3 seperatingAxisInB = -v * 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 ); SimdPoint3 w = pWorld - qWorld; delta = v.dot( w ); // Keep tighest upper bound upperBoundSqrd = SimdMin( upperBoundSqrd, delta * delta / vSqrd ); //assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" ); isCloseEnough = ( upperBoundSqrd <= ( 1 + 1e-4f ) * vSqrd ); if ( !isCloseEnough ) { std::list< EpaFace* > newFaces; bool expandOk = m_polyhedron.Expand( w, pWorld, qWorld, pEpaFace, newFaces ); if ( expandOk ) { EPA_DEBUG_ASSERT( !newFaces.empty() ,"EPA polyhedron not expanding ?" ); bool check = true; bool areEqual = false; while ( !newFaces.empty() ) { EpaFace* pNewFace = newFaces.front(); EPA_DEBUG_ASSERT( !pNewFace->m_deleted ,"New face is deleted!" ); if ( !pNewFace->m_deleted ) { EPA_DEBUG_ASSERT( ( pNewFace->m_vSqrd > 0 ) ,"Face containing the origin!" ); EPA_DEBUG_ASSERT( !pNewFace->IsAffinelyDependent() ,"Face is affinely dependent!" ); //#ifdef EPA_POLYHEDRON_USE_PLANES //// if ( pNewFace->m_planeDistance >= 0 ) //// { // // assert( false && "Face's plane distance greater than 0!" ); //#ifdef _DEBUG //// m_polyhedron._dbgSaveToFile( "epa_beforeFix.dbg" ); //#endif // //pNewFace->FixOrder(); //#ifdef _DEBUG // //m_polyhedron._dbgSaveToFile( "epa_afterFix.dbg" ); //#endif //// } //#endif // //#ifdef EPA_POLYHEDRON_USE_PLANES // //assert( ( pNewFace->m_planeDistance < 0 ) && "Face's plane distance equal or greater than 0!" ); //#endif if ( pNewFace->IsClosestPointInternal() && ( vSqrd <= pNewFace->m_vSqrd ) && ( pNewFace->m_vSqrd <= upperBoundSqrd ) ) { m_faceEntries.push_back( pNewFace ); std::push_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries ); } } newFaces.pop_front(); } } else { pEpaFace->CalcClosestPointOnA( wWitnessOnA ); pEpaFace->CalcClosestPointOnB( wWitnessOnB ); #ifdef _DEBUG //m_polyhedron._dbgSaveToFile( "epa_end.dbg" ); #endif return v.length(); } } } ++nbIterations; } while ( ( m_polyhedron.GetNbFaces() < EPA_MAX_FACE_ENTRIES ) &&/*( nbIterations < nbMaxIterations ) &&*/ !isCloseEnough && ( m_faceEntries.size() > 0 ) && ( m_faceEntries[ 0 ]->m_vSqrd <= upperBoundSqrd ) ); #ifdef _DEBUG //m_polyhedron._dbgSaveToFile( "epa_end.dbg" ); #endif EPA_DEBUG_ASSERT( pEpaFace ,"Invalid epa face!" ); pEpaFace->CalcClosestPointOnA( wWitnessOnA ); pEpaFace->CalcClosestPointOnB( wWitnessOnB ); return v.length(); }
bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints, SimdPoint3* pSupportPointsOnA, SimdPoint3* pSupportPointsOnB, const int nbInitialPoints ) { #ifndef EPA_POLYHEDRON_USE_PLANES assert( ( nbInitialPoints <= 4 ) && "nbInitialPoints greater than 4!" ); #endif if ( nbInitialPoints < 4 ) { // Insufficient nb of points return false; } //////////////////////////////////////////////////////////////////////////////// #ifdef EPA_POLYHEDRON_USE_PLANES int nbDiffCoords[ 3 ] = { 0, 0, 0 }; bool* pDiffCoords = new bool[ 3 * nbInitialPoints ]; int i; for (i=0;i<nbInitialPoints*3;i++) { pDiffCoords[i] = false; } //::memset( pDiffCoords, 0, sizeof( bool ) * 3 * nbInitialPoints ); int axis; for ( axis = 0; axis < 3; ++axis ) { for ( int i = 0; i < nbInitialPoints; ++i ) { bool isDifferent = true; for ( int j = 0; j < i; ++j ) { if ( pInitialPoints[ i ][ axis ] == pInitialPoints[ j ][ axis ] ) { isDifferent = false; break; } } if ( isDifferent ) { ++nbDiffCoords[ axis ]; pDiffCoords[ axis * nbInitialPoints + i ] = true; } } if ( nbDiffCoords[ axis ] <= 1 ) { // The input is degenerate return false; } } int finalPointsIndices[ 4 ] = { -1, -1, -1, -1 }; int axisOrderIndices[ 3 ] = { 0, 1, 2 }; for ( i = 0; i < 2/*round( nbAxis / 2 )*/; ++i ) { if ( nbDiffCoords[ i ] > nbDiffCoords[ i + 1 ] ) { int tmp = nbDiffCoords[ i ]; nbDiffCoords[ i ] = nbDiffCoords[ i + 1 ]; nbDiffCoords[ i + 1 ] = tmp; tmp = axisOrderIndices[ i ]; axisOrderIndices[ i ] = axisOrderIndices[ i + 1 ]; axisOrderIndices[ i + 1 ] = tmp; } } int nbSuccessfullAxis = 0; // The axes with less different coordinates choose first int minsIndices[ 3 ] = { -1, -1, -1 }; int maxsIndices[ 3 ] = { -1, -1, -1 }; int finalPointsIndex = 0; for ( axis = 0; ( axis < 3 ) && ( nbSuccessfullAxis < 2 ); ++axis ) { int axisIndex = axisOrderIndices[ axis ]; SimdScalar axisMin = SIMD_INFINITY; SimdScalar axisMax = -SIMD_INFINITY; for ( int i = 0; i < 4; ++i ) { // Among the diff coords pick the min and max coords if ( pDiffCoords[ axisIndex * nbInitialPoints + i ] ) { if ( pInitialPoints[ i ][ axisIndex ] < axisMin ) { axisMin = pInitialPoints[ i ][ axisIndex ]; minsIndices[ axisIndex ] = i; } if ( pInitialPoints[ i ][ axisIndex ] > axisMax ) { axisMax = pInitialPoints[ i ][ axisIndex ]; maxsIndices[ axisIndex ] = i; } } } //assert( ( minsIndices[ axisIndex ] != maxsIndices[ axisIndex ] ) && // "min and max have the same index!" ); if ( ( minsIndices[ axisIndex ] != -1 ) && ( maxsIndices[ axisIndex ] != -1 ) && ( minsIndices[ axisIndex ] != maxsIndices[ axisIndex ] ) ) { ++nbSuccessfullAxis; finalPointsIndices[ finalPointsIndex++ ] = minsIndices[ axisIndex ]; finalPointsIndices[ finalPointsIndex++ ] = maxsIndices[ axisIndex ]; // Make the choosen points to be impossible for other axes to choose //assert( ( minsIndices[ axisIndex ] != -1 ) && "Invalid index!" ); //assert( ( maxsIndices[ axisIndex ] != -1 ) && "Invalid index!" ); for ( int i = 0; i < 3; ++i ) { pDiffCoords[ i * nbInitialPoints + minsIndices[ axisIndex ] ] = false; pDiffCoords[ i * nbInitialPoints + maxsIndices[ axisIndex ] ] = false; } } } if ( nbSuccessfullAxis <= 1 ) { // Degenerate input ? assert( false && "nbSuccessfullAxis must be greater than 1!" ); return false; } delete[] pDiffCoords; #endif ////////////////////////////////////////////////////////////////////////// #ifdef EPA_POLYHEDRON_USE_PLANES SimdVector3 v0 = pInitialPoints[ finalPointsIndices[ 1 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ]; SimdVector3 v1 = pInitialPoints[ finalPointsIndices[ 2 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ]; #else SimdVector3 v0 = pInitialPoints[ 1 ] - pInitialPoints[ 0 ]; SimdVector3 v1 = pInitialPoints[ 2 ] - pInitialPoints[ 0 ]; #endif SimdVector3 planeNormal = v1.cross( v0 ); planeNormal.normalize(); #ifdef EPA_POLYHEDRON_USE_PLANES SimdScalar planeDistance = pInitialPoints[ finalPointsIndices[ 0 ] ].dot( -planeNormal ); #else SimdScalar planeDistance = pInitialPoints[ 0 ].dot( -planeNormal ); #endif #ifdef EPA_POLYHEDRON_USE_PLANES assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 0 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) && "Point should be on plane!" ); assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 1 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) && "Point should be on plane!" ); assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 2 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) && "Point should be on plane!" ); #endif #ifndef EPA_POLYHEDRON_USE_PLANES { if ( planeDistance > 0 ) { SimdVector3 tmp = pInitialPoints[ 1 ]; pInitialPoints[ 1 ] = pInitialPoints[ 2 ]; pInitialPoints[ 2 ] = tmp; tmp = pSupportPointsOnA[ 1 ]; pSupportPointsOnA[ 1 ] = pSupportPointsOnA[ 2 ]; pSupportPointsOnA[ 2 ] = tmp; tmp = pSupportPointsOnB[ 1 ]; pSupportPointsOnB[ 1 ] = pSupportPointsOnB[ 2 ]; pSupportPointsOnB[ 2 ] = tmp; } } EpaVertex* pVertexA = CreateVertex( pInitialPoints[ 0 ], pSupportPointsOnA[ 0 ], pSupportPointsOnB[ 0 ] ); EpaVertex* pVertexB = CreateVertex( pInitialPoints[ 1 ], pSupportPointsOnA[ 1 ], pSupportPointsOnB[ 1 ] ); EpaVertex* pVertexC = CreateVertex( pInitialPoints[ 2 ], pSupportPointsOnA[ 2 ], pSupportPointsOnB[ 2 ] ); EpaVertex* pVertexD = CreateVertex( pInitialPoints[ 3 ], pSupportPointsOnA[ 3 ], pSupportPointsOnB[ 3 ] ); #else finalPointsIndices[ 3 ] = -1; SimdScalar absMaxDist = -SIMD_INFINITY; SimdScalar maxDist; for ( int pointIndex = 0; pointIndex < nbInitialPoints; ++pointIndex ) { SimdScalar dist = planeNormal.dot( pInitialPoints[ pointIndex ] ) + planeDistance; SimdScalar absDist = abs( dist ); if ( ( absDist > absMaxDist ) && !SimdEqual( dist, PLANE_THICKNESS ) ) { absMaxDist = absDist; maxDist = dist; finalPointsIndices[ 3 ] = pointIndex; } } if ( finalPointsIndices[ 3 ] == -1 ) { Destroy(); return false; } if ( maxDist > PLANE_THICKNESS ) { // Can swap indices only SimdPoint3 tmp = pInitialPoints[ finalPointsIndices[ 1 ] ]; pInitialPoints[ finalPointsIndices[ 1 ] ] = pInitialPoints[ finalPointsIndices[ 2 ] ]; pInitialPoints[ finalPointsIndices[ 2 ] ] = tmp; tmp = pSupportPointsOnA[ finalPointsIndices[ 1 ] ]; pSupportPointsOnA[ finalPointsIndices[ 1 ] ] = pSupportPointsOnA[ finalPointsIndices[ 2 ] ]; pSupportPointsOnA[ finalPointsIndices[ 2 ] ] = tmp; tmp = pSupportPointsOnB[ finalPointsIndices[ 1 ] ]; pSupportPointsOnB[ finalPointsIndices[ 1 ] ] = pSupportPointsOnB[ finalPointsIndices[ 2 ] ]; pSupportPointsOnB[ finalPointsIndices[ 2 ] ] = tmp; } EpaVertex* pVertexA = CreateVertex( pInitialPoints[ finalPointsIndices[ 0 ] ], pSupportPointsOnA[ finalPointsIndices[ 0 ] ], pSupportPointsOnB[ finalPointsIndices[ 0 ] ] ); EpaVertex* pVertexB = CreateVertex( pInitialPoints[ finalPointsIndices[ 1 ] ], pSupportPointsOnA[ finalPointsIndices[ 1 ] ], pSupportPointsOnB[ finalPointsIndices[ 1 ] ] ); EpaVertex* pVertexC = CreateVertex( pInitialPoints[ finalPointsIndices[ 2 ] ], pSupportPointsOnA[ finalPointsIndices[ 2 ] ], pSupportPointsOnB[ finalPointsIndices[ 2 ] ] ); EpaVertex* pVertexD = CreateVertex( pInitialPoints[ finalPointsIndices[ 3 ] ], pSupportPointsOnA[ finalPointsIndices[ 3 ] ], pSupportPointsOnB[ finalPointsIndices[ 3 ] ] ); #endif EpaFace* pFaceA = CreateFace(); EpaFace* pFaceB = CreateFace(); EpaFace* pFaceC = CreateFace(); EpaFace* pFaceD = CreateFace(); EpaHalfEdge* pFaceAHalfEdges[ 3 ]; EpaHalfEdge* pFaceCHalfEdges[ 3 ]; EpaHalfEdge* pFaceBHalfEdges[ 3 ]; EpaHalfEdge* pFaceDHalfEdges[ 3 ]; pFaceAHalfEdges[ 0 ] = CreateHalfEdge(); pFaceAHalfEdges[ 1 ] = CreateHalfEdge(); pFaceAHalfEdges[ 2 ] = CreateHalfEdge(); pFaceBHalfEdges[ 0 ] = CreateHalfEdge(); pFaceBHalfEdges[ 1 ] = CreateHalfEdge(); pFaceBHalfEdges[ 2 ] = CreateHalfEdge(); pFaceCHalfEdges[ 0 ] = CreateHalfEdge(); pFaceCHalfEdges[ 1 ] = CreateHalfEdge(); pFaceCHalfEdges[ 2 ] = CreateHalfEdge(); pFaceDHalfEdges[ 0 ] = CreateHalfEdge(); pFaceDHalfEdges[ 1 ] = CreateHalfEdge(); pFaceDHalfEdges[ 2 ] = CreateHalfEdge(); pFaceA->m_pHalfEdge = pFaceAHalfEdges[ 0 ]; pFaceB->m_pHalfEdge = pFaceBHalfEdges[ 0 ]; pFaceC->m_pHalfEdge = pFaceCHalfEdges[ 0 ]; pFaceD->m_pHalfEdge = pFaceDHalfEdges[ 0 ]; pFaceAHalfEdges[ 0 ]->m_pNextCCW = pFaceAHalfEdges[ 1 ]; pFaceAHalfEdges[ 1 ]->m_pNextCCW = pFaceAHalfEdges[ 2 ]; pFaceAHalfEdges[ 2 ]->m_pNextCCW = pFaceAHalfEdges[ 0 ]; pFaceBHalfEdges[ 0 ]->m_pNextCCW = pFaceBHalfEdges[ 1 ]; pFaceBHalfEdges[ 1 ]->m_pNextCCW = pFaceBHalfEdges[ 2 ]; pFaceBHalfEdges[ 2 ]->m_pNextCCW = pFaceBHalfEdges[ 0 ]; pFaceCHalfEdges[ 0 ]->m_pNextCCW = pFaceCHalfEdges[ 1 ]; pFaceCHalfEdges[ 1 ]->m_pNextCCW = pFaceCHalfEdges[ 2 ]; pFaceCHalfEdges[ 2 ]->m_pNextCCW = pFaceCHalfEdges[ 0 ]; pFaceDHalfEdges[ 0 ]->m_pNextCCW = pFaceDHalfEdges[ 1 ]; pFaceDHalfEdges[ 1 ]->m_pNextCCW = pFaceDHalfEdges[ 2 ]; pFaceDHalfEdges[ 2 ]->m_pNextCCW = pFaceDHalfEdges[ 0 ]; pFaceAHalfEdges[ 0 ]->m_pFace = pFaceA; pFaceAHalfEdges[ 1 ]->m_pFace = pFaceA; pFaceAHalfEdges[ 2 ]->m_pFace = pFaceA; pFaceBHalfEdges[ 0 ]->m_pFace = pFaceB; pFaceBHalfEdges[ 1 ]->m_pFace = pFaceB; pFaceBHalfEdges[ 2 ]->m_pFace = pFaceB; pFaceCHalfEdges[ 0 ]->m_pFace = pFaceC; pFaceCHalfEdges[ 1 ]->m_pFace = pFaceC; pFaceCHalfEdges[ 2 ]->m_pFace = pFaceC; pFaceDHalfEdges[ 0 ]->m_pFace = pFaceD; pFaceDHalfEdges[ 1 ]->m_pFace = pFaceD; pFaceDHalfEdges[ 2 ]->m_pFace = pFaceD; pFaceAHalfEdges[ 0 ]->m_pVertex = pVertexA; pFaceAHalfEdges[ 1 ]->m_pVertex = pVertexB; pFaceAHalfEdges[ 2 ]->m_pVertex = pVertexC; pFaceBHalfEdges[ 0 ]->m_pVertex = pVertexB; pFaceBHalfEdges[ 1 ]->m_pVertex = pVertexD; pFaceBHalfEdges[ 2 ]->m_pVertex = pVertexC; pFaceCHalfEdges[ 0 ]->m_pVertex = pVertexD; pFaceCHalfEdges[ 1 ]->m_pVertex = pVertexA; pFaceCHalfEdges[ 2 ]->m_pVertex = pVertexC; pFaceDHalfEdges[ 0 ]->m_pVertex = pVertexB; pFaceDHalfEdges[ 1 ]->m_pVertex = pVertexA; pFaceDHalfEdges[ 2 ]->m_pVertex = pVertexD; //pVertexA->m_pHalfEdge = pFaceAHalfEdges[ 0 ]; //pVertexB->m_pHalfEdge = pFaceAHalfEdges[ 1 ]; //pVertexC->m_pHalfEdge = pFaceAHalfEdges[ 2 ]; //pVertexD->m_pHalfEdge = pFaceBHalfEdges[ 1 ]; pFaceAHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 0 ]; pFaceAHalfEdges[ 1 ]->m_pTwin = pFaceBHalfEdges[ 2 ]; pFaceAHalfEdges[ 2 ]->m_pTwin = pFaceCHalfEdges[ 1 ]; pFaceBHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 2 ]; pFaceBHalfEdges[ 1 ]->m_pTwin = pFaceCHalfEdges[ 2 ]; pFaceBHalfEdges[ 2 ]->m_pTwin = pFaceAHalfEdges[ 1 ]; pFaceCHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 1 ]; pFaceCHalfEdges[ 1 ]->m_pTwin = pFaceAHalfEdges[ 2 ]; pFaceCHalfEdges[ 2 ]->m_pTwin = pFaceBHalfEdges[ 1 ]; pFaceDHalfEdges[ 0 ]->m_pTwin = pFaceAHalfEdges[ 0 ]; pFaceDHalfEdges[ 1 ]->m_pTwin = pFaceCHalfEdges[ 0 ]; pFaceDHalfEdges[ 2 ]->m_pTwin = pFaceBHalfEdges[ 0 ]; if ( !pFaceA->Initialize() || !pFaceB->Initialize() || !pFaceC->Initialize() || !pFaceD->Initialize() ) { assert( false && "One initial face failed to initialize!" ); return false; } #ifdef EPA_POLYHEDRON_USE_PLANES if ( nbInitialPoints > 4 ) { for ( int i = 0; i < nbInitialPoints; ++i ) { if ( ( i != finalPointsIndices[ 0 ] ) && ( i != finalPointsIndices[ 1 ] ) && ( i != finalPointsIndices[ 2 ] ) && ( i != finalPointsIndices[ 3 ] ) ) { std::list< EpaFace* >::iterator facesItr( m_faces.begin() ); while ( facesItr != m_faces.end() ) { EpaFace* pFace = *facesItr; SimdScalar dist = pFace->m_planeNormal.dot( pInitialPoints[ i ] ) + pFace->m_planeDistance; if ( dist > PLANE_THICKNESS ) { std::list< EpaFace* > newFaces; bool expandOk = Expand( pInitialPoints[ i ], pSupportPointsOnA[ i ], pSupportPointsOnB[ i ], pFace, newFaces ); if ( !expandOk ) { // One or more new faces are affinely dependent return false; } assert( !newFaces.empty() && "Polyhedron should have expanded!" ); break; } ++facesItr; } } } } #endif return true; }
bool VoronoiSimplexSolver::UpdateClosestVectorAndPoints() { if (m_needsUpdate) { m_cachedBC.Reset(); m_needsUpdate = false; switch (numVertices()) { case 0: m_cachedValidClosest = false; break; case 1: { m_cachedP1 = m_simplexPointsP[0]; m_cachedP2 = m_simplexPointsQ[0]; m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0] m_cachedBC.Reset(); m_cachedBC.SetBarycentricCoordinates(1.f,0.f,0.f,0.f); m_cachedValidClosest = m_cachedBC.IsValid(); break; }; case 2: { //closest point origin from line segment const SimdVector3& from = m_simplexVectorW[0]; const SimdVector3& to = m_simplexVectorW[1]; SimdVector3 nearest; SimdVector3 p (0.f,0.f,0.f); SimdVector3 diff = p - from; SimdVector3 v = to - from; float t = v.dot(diff); if (t > 0) { float dotVV = v.dot(v); if (t < dotVV) { t /= dotVV; diff -= t*v; m_cachedBC.m_usedVertices.usedVertexA = true; m_cachedBC.m_usedVertices.usedVertexB = true; } else { t = 1; diff -= v; //reduce to 1 point m_cachedBC.m_usedVertices.usedVertexB = true; } } else { t = 0; //reduce to 1 point m_cachedBC.m_usedVertices.usedVertexA = true; } m_cachedBC.SetBarycentricCoordinates(1-t,t); nearest = from + t*v; m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]); m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]); m_cachedV = m_cachedP1 - m_cachedP2; ReduceVertices(m_cachedBC.m_usedVertices); m_cachedValidClosest = m_cachedBC.IsValid(); break; } case 3: { //closest point origin from triangle SimdVector3 p (0.f,0.f,0.f); const SimdVector3& a = m_simplexVectorW[0]; const SimdVector3& b = m_simplexVectorW[1]; const SimdVector3& c = m_simplexVectorW[2]; ClosestPtPointTriangle(p,a,b,c,m_cachedBC); m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; m_cachedV = m_cachedP1-m_cachedP2; ReduceVertices (m_cachedBC.m_usedVertices); m_cachedValidClosest = m_cachedBC.IsValid(); break; } case 4: { SimdVector3 p (0.f,0.f,0.f); const SimdVector3& a = m_simplexVectorW[0]; const SimdVector3& b = m_simplexVectorW[1]; const SimdVector3& c = m_simplexVectorW[2]; const SimdVector3& d = m_simplexVectorW[3]; bool hasSeperation = ClosestPtPointTetrahedron(p,a,b,c,d,m_cachedBC); if (hasSeperation) { m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; m_cachedV = m_cachedP1-m_cachedP2; ReduceVertices (m_cachedBC.m_usedVertices); } else { // printf("sub distance got penetration\n"); if (m_cachedBC.m_degenerate) { m_cachedValidClosest = false; } else { m_cachedValidClosest = true; //degenerate case == false, penetration = true + zero m_cachedV.setValue(0.f,0.f,0.f); } break; } m_cachedValidClosest = m_cachedBC.IsValid(); //closest point origin from tetrahedron break; } default: { m_cachedValidClosest = false; } }; } return m_cachedValidClosest; }
bool VoronoiSimplexSolver::ClosestPtPointTriangle(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c,SubSimplexClosestResult& result) { result.m_usedVertices.reset(); // Check if P in vertex region outside A SimdVector3 ab = b - a; SimdVector3 ac = c - a; SimdVector3 ap = p - a; float d1 = ab.dot(ap); float d2 = ac.dot(ap); if (d1 <= 0.0f && d2 <= 0.0f) { result.m_closestPointOnSimplex = a; result.m_usedVertices.usedVertexA = true; result.SetBarycentricCoordinates(1,0,0); return true;// a; // barycentric coordinates (1,0,0) } // Check if P in vertex region outside B SimdVector3 bp = p - b; float d3 = ab.dot(bp); float d4 = ac.dot(bp); if (d3 >= 0.0f && d4 <= d3) { result.m_closestPointOnSimplex = b; result.m_usedVertices.usedVertexB = true; result.SetBarycentricCoordinates(0,1,0); return true; // b; // barycentric coordinates (0,1,0) } // Check if P in edge region of AB, if so return projection of P onto AB float vc = d1*d4 - d3*d2; if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) { float v = d1 / (d1 - d3); result.m_closestPointOnSimplex = a + v * ab; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexB = true; result.SetBarycentricCoordinates(1-v,v,0); return true; //return a + v * ab; // barycentric coordinates (1-v,v,0) } // Check if P in vertex region outside C SimdVector3 cp = p - c; float d5 = ab.dot(cp); float d6 = ac.dot(cp); if (d6 >= 0.0f && d5 <= d6) { result.m_closestPointOnSimplex = c; result.m_usedVertices.usedVertexC = true; result.SetBarycentricCoordinates(0,0,1); return true;//c; // barycentric coordinates (0,0,1) } // Check if P in edge region of AC, if so return projection of P onto AC float vb = d5*d2 - d1*d6; if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) { float w = d2 / (d2 - d6); result.m_closestPointOnSimplex = a + w * ac; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexC = true; result.SetBarycentricCoordinates(1-w,0,w); return true; //return a + w * ac; // barycentric coordinates (1-w,0,w) } // Check if P in edge region of BC, if so return projection of P onto BC float va = d3*d6 - d5*d4; if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) { float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); result.m_closestPointOnSimplex = b + w * (c - b); result.m_usedVertices.usedVertexB = true; result.m_usedVertices.usedVertexC = true; result.SetBarycentricCoordinates(0,1-w,w); return true; // return b + w * (c - b); // barycentric coordinates (0,1-w,w) } // P inside face region. Compute Q through its barycentric coordinates (u,v,w) float denom = 1.0f / (va + vb + vc); float v = vb * denom; float w = vc * denom; result.m_closestPointOnSimplex = a + ab * v + ac * w; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexB = true; result.m_usedVertices.usedVertexC = true; result.SetBarycentricCoordinates(1-v-w,v,w); return true; // return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0f - v - w }