bool EpaPenetrationDepthSolver::CalcPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdVector3& v, SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
class IDebugDraw* debugDraw )
{
EPA_DEBUG_ASSERT( pConvexA ,"Convex shape A is invalid!" );
EPA_DEBUG_ASSERT( pConvexB ,"Convex shape B is invalid!" );
SimdScalar penDepth;
#ifdef EPA_USE_HYBRID
bool needsEPA = !HybridPenDepth( simplexSolver, pConvexA, pConvexB, transformA, transformB,
wWitnessOnA, wWitnessOnB, penDepth, v );
if ( needsEPA )
{
#endif
penDepth = EpaPenDepth( simplexSolver, pConvexA, pConvexB,
transformA, transformB,
wWitnessOnA, wWitnessOnB );
EPA_DEBUG_ASSERT( ( penDepth > 0 ) ,"EPA or Hybrid Technique failed to calculate penetration depth!" );
#ifdef EPA_USE_HYBRID
}
#endif
return ( penDepth > 0 );
}
void EpaPolyhedron::CreateCone( EpaVertex* pAppexVertex, std::list< EpaHalfEdge* >& baseTwinHalfEdges,
std::list< EpaFace* >& newFaces )
{
EPA_DEBUG_ASSERT( ( baseTwinHalfEdges.size() >= 3 ) ,"DeleteVisibleFaces method didn't do its job right!" );
std::list< EpaHalfEdge* >::iterator baseHalfEdgesItr( baseTwinHalfEdges.begin() );
std::list< EpaHalfEdge* > halfEdgesToLink;
while ( baseHalfEdgesItr != baseTwinHalfEdges.end() )
{
EpaFace* pNewFace = CreateConeFace( pAppexVertex, *baseHalfEdgesItr, halfEdgesToLink );
newFaces.push_back( pNewFace );
++baseHalfEdgesItr;
}
// Connect consecutive faces by linking twin half-edges
EPA_DEBUG_ASSERT( ( halfEdgesToLink.size() % 2 == 0 ) ,"Nb half-edges to link is odd!" );
int nbLinksToCreate = halfEdgesToLink.size() / 2;
int nbLinksCreated = 0;
std::list< EpaHalfEdge* >::iterator halfEdgesItr( halfEdgesToLink.begin() );
for ( ; ( halfEdgesItr != halfEdgesToLink.end() ) && ( nbLinksCreated < nbLinksToCreate ); ++halfEdgesItr )
{
std::list< EpaHalfEdge* >::iterator halfEdgesItr2( halfEdgesItr );
++halfEdgesItr2;
for ( ; ( halfEdgesItr2 != halfEdgesToLink.end() ) && ( nbLinksCreated < nbLinksToCreate ); ++halfEdgesItr2 )
{
EpaHalfEdge* pHalfEdge1 = *halfEdgesItr;
EpaHalfEdge* pHalfEdge2 = *halfEdgesItr2;
EpaHalfEdge* pHalfEdgeNextCCW1 = pHalfEdge1->m_pNextCCW;
EpaHalfEdge* pHalfEdgeNextCCW2 = pHalfEdge2->m_pNextCCW;
if ( ( pHalfEdge2->m_pVertex == pHalfEdgeNextCCW1->m_pVertex ) &&
( pHalfEdgeNextCCW2->m_pVertex == pHalfEdge1->m_pVertex ) )
{
pHalfEdge1->m_pTwin = pHalfEdge2;
pHalfEdge2->m_pTwin = pHalfEdge1;
++nbLinksCreated;
}
}
}
EPA_DEBUG_ASSERT( ( nbLinksCreated == nbLinksToCreate ) ,"Mesh topology not ok!" );
}
bool EpaPolyhedron::Expand( const btPoint3& wSupportPoint,
const btPoint3& wSupportPointOnA,
const btPoint3& wSupportPointOnB,
EpaFace* pFace, std::list< EpaFace* >& newFaces )
{
EPA_DEBUG_ASSERT( !pFace->m_deleted ,"Face is already deleted!" );
EPA_DEBUG_ASSERT( newFaces.empty() ,"NewFaces list must be empty!" );
EPA_DEBUG_ASSERT( !pFace->m_deleted ,"Cannot expand deleted face!" );
// wSupportPoint must be front of face's plane used to do the expansion
#ifdef EPA_POLYHEDRON_USE_PLANES
btScalar dist = pFace->m_planeNormal.dot( wSupportPoint ) + pFace->m_planeDistance;
if ( dist <= PLANE_THICKNESS )
{
return false;
}
#endif
std::list< EpaHalfEdge* > coneBaseEdges;
DeleteVisibleFaces( wSupportPoint, pFace, coneBaseEdges );
EPA_DEBUG_ASSERT( ( coneBaseEdges.size() >= 3 ) ,"Cone base must have at least 3 edges!" );
EpaVertex* pConeAppex = CreateVertex( wSupportPoint, wSupportPointOnA, wSupportPointOnB );
EPA_DEBUG_ASSERT( pConeAppex ,"Failed to create vertex!" );
CreateCone( pConeAppex, coneBaseEdges, newFaces );
// Initialize new faces
std::list< EpaFace* >::iterator newFacesItr( newFaces.begin() );
while ( newFacesItr != newFaces.end() )
{
EpaFace* pNewFace = *newFacesItr;
if ( !pNewFace->Initialize() )
{
return false;
}
++newFacesItr;
}
return true;
}
EpaHalfEdge* EpaPolyhedron::CreateHalfEdge()
{
EpaHalfEdge* pNewHalfEdge = new EpaHalfEdge();
EPA_DEBUG_ASSERT( pNewHalfEdge ,"Failed to allocate memory for a new EpaHalfEdge!" );
m_halfEdges.push_back( pNewHalfEdge );
return pNewHalfEdge;
}
void EpaPolyhedron::DeleteVisibleFaces( const btPoint3& point, EpaFace* pFace,
std::list< EpaHalfEdge* >& coneBaseTwinHalfEdges )
{
EPA_DEBUG_ASSERT( !pFace->m_deleted ,"Face is already deleted!" );
DeleteFace( pFace );
EpaHalfEdge* pCurrentHalfEdge = pFace->m_pHalfEdge;
do
{
EPA_DEBUG_ASSERT( pCurrentHalfEdge->m_pTwin ,"Half-edge without a twin!" );
EpaFace* pAdjacentFace = pCurrentHalfEdge->m_pTwin->m_pFace;
EPA_DEBUG_ASSERT( pAdjacentFace ,"Invalid adjacent face!" );
if ( !pAdjacentFace->m_deleted )
{
#ifdef EPA_POLYHEDRON_USE_PLANES
EPA_DEBUG_ASSERT( ( pAdjacentFace->m_planeNormal.length2() > 0 ) ,"Invalid plane!" );
btScalar pointDist = pAdjacentFace->m_planeNormal.dot( point ) +
pAdjacentFace->m_planeDistance;
if ( pointDist > PLANE_THICKNESS )
#else
btScalar dot = pAdjacentFace->m_v.dot( point );
if ( dot >= pAdjacentFace->m_vSqrd )
#endif
{
DeleteVisibleFaces( point, pAdjacentFace, coneBaseTwinHalfEdges );
}
else
{
coneBaseTwinHalfEdges.push_back( pCurrentHalfEdge->m_pTwin );
}
}
pCurrentHalfEdge = pCurrentHalfEdge->m_pNextCCW;
}
while( pCurrentHalfEdge != pFace->m_pHalfEdge );
}
EpaVertex* EpaPolyhedron::CreateVertex( const btPoint3& wSupportPoint,
const btPoint3& wSupportPointOnA,
const btPoint3& wSupportPointOnB )
{
EpaVertex* pNewVertex = new EpaVertex( wSupportPoint, wSupportPointOnA, wSupportPointOnB );
EPA_DEBUG_ASSERT( pNewVertex ,"Failed to allocate memory for a new EpaVertex!" );
m_vertices.push_back( pNewVertex );
return pNewVertex;
}
EpaFace* EpaPolyhedron::CreateFace()
{
EpaFace* pNewFace = new EpaFace();
EPA_DEBUG_ASSERT( pNewFace , "Failed to allocate memory for a new EpaFace!" );
m_faces.push_back( pNewFace );
++m_nbFaces;
return pNewFace;
}
SimdScalar EpaPenetrationDepthSolver::EpaPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
{
Epa epa( pConvexA, pConvexB, transformA, transformB );
if ( !epa.Initialize( simplexSolver ) )
{
EPA_DEBUG_ASSERT( false ,"Epa failed to initialize!" );
return 0;
}
return epa.CalcPenDepth( wWitnessOnA, wWitnessOnB );
}
bool EpaPolyhedron::Create( btPoint3* pInitialPoints,
btPoint3* pSupportPointsOnA, btPoint3* pSupportPointsOnB,
const int nbInitialPoints )
{
#ifndef EPA_POLYHEDRON_USE_PLANES
EPA_DEBUG_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 ];
btScalar axisMin = SIMD_INFINITY;
btScalar 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 ?
EPA_DEBUG_ASSERT( false ,"nbSuccessfullAxis must be greater than 1!" );
return false;
}
delete[] pDiffCoords;
#endif
//////////////////////////////////////////////////////////////////////////
#ifdef EPA_POLYHEDRON_USE_PLANES
btVector3 v0 = pInitialPoints[ finalPointsIndices[ 1 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
btVector3 v1 = pInitialPoints[ finalPointsIndices[ 2 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
#else
btVector3 v0 = pInitialPoints[ 1 ] - pInitialPoints[ 0 ];
btVector3 v1 = pInitialPoints[ 2 ] - pInitialPoints[ 0 ];
#endif
btVector3 planeNormal = v1.cross( v0 );
planeNormal.normalize();
#ifdef EPA_POLYHEDRON_USE_PLANES
btScalar planeDistance = pInitialPoints[ finalPointsIndices[ 0 ] ].dot( -planeNormal );
#else
btScalar planeDistance = pInitialPoints[ 0 ].dot( -planeNormal );
#endif
#ifdef EPA_POLYHEDRON_USE_PLANES
bool pointOnPlane0 = btEqual( pInitialPoints[ finalPointsIndices[ 0 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane0)
{
EPA_DEBUG_ASSERT(0,"Point0 should be on plane!");
return false;
}
bool pointOnPlane1 = btEqual( pInitialPoints[ finalPointsIndices[ 1 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane1)
{
EPA_DEBUG_ASSERT(0,"Point1 should be on plane!");
return false;
}
bool pointOnPlane2 = btEqual( pInitialPoints[ finalPointsIndices[ 2 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane2)
{
EPA_DEBUG_ASSERT(0,"Point2 should be on plane!");
return false;
}
#endif
#ifndef EPA_POLYHEDRON_USE_PLANES
{
if ( planeDistance > 0 )
{
btVector3 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;
btScalar absMaxDist = -SIMD_INFINITY;
btScalar maxDist;
for ( int pointIndex = 0; pointIndex < nbInitialPoints; ++pointIndex )
{
btScalar dist = planeNormal.dot( pInitialPoints[ pointIndex ] ) + planeDistance;
btScalar absDist = abs( dist );
if ( ( absDist > absMaxDist ) &&
!btEqual( 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
btPoint3 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() )
{
EPA_DEBUG_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;
btScalar 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;
}
EPA_DEBUG_ASSERT( !newFaces.empty() ,"Polyhedron should have expanded!" );
break;
}
++facesItr;
}
}
}
}
#endif
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;
}
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();
}
