bool calcPenDepth() { // Ryn Hybrid Pen Depth and EPA if necessary SimdVector3 v( 1, 0, 0 ); SimdScalar squaredDistance = SIMD_INFINITY; SimdScalar delta = 0.f; const SimdScalar margin = g_pConvexShapes[ 0 ]->GetMargin() + g_pConvexShapes[ 1 ]->GetMargin(); const SimdScalar marginSqrd = margin * margin; SimdScalar maxRelErrorSqrd = 1e-3 * 1e-3; simplexSolver.reset(); while ( true ) { assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" ); SimdVector3 seperatingAxisInA = -v * g_convexShapesTransform[ 0 ].getBasis(); SimdVector3 seperatingAxisInB = v * g_convexShapesTransform[ 1 ].getBasis(); SimdVector3 pInA = g_pConvexShapes[ 0 ]->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA ); SimdVector3 qInB = g_pConvexShapes[ 1 ]->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB ); SimdPoint3 pWorld = g_convexShapesTransform[ 0 ]( pInA ); SimdPoint3 qWorld = g_convexShapesTransform[ 1 ]( 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 return false; } //exit 0: the new point is already in the simplex, or we didn't come any closer if ( ( squaredDistance - delta <= squaredDistance * maxRelErrorSqrd ) || simplexSolver.inSimplex( w ) ) { simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] ); assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); SimdScalar vLength = sqrt( squaredDistance ); g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->GetMargin() / vLength ); g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength ); 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( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] ); assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); SimdScalar vLength = sqrt( squaredDistance ); g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->GetMargin() / vLength ); g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength ); 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( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] ); assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); SimdScalar vLength = sqrt( squaredDistance ); g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->GetMargin() / vLength ); g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength ); return true; } if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) ) { // Run EPA break; } } return epaPenDepthSolver.CalcPenDepth( simplexSolver, g_pConvexShapes[ 0 ], g_pConvexShapes[ 1 ], g_convexShapesTransform[ 0 ], g_convexShapesTransform[ 1 ], v, g_wWitnesses[ 0 ], g_wWitnesses[ 1 ], 0 ); }
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; } }
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; }