Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

tested Epa, and added some debug printf stuff

Browse Source 
Still has some issues, but so does the sampling MinkowskisumPenetrationDepth solver (its an approximation)
This commit is contained in:
ejcoumans
2006-09-07 22:55:17 +00:00
parent 1bbd779aa3
commit 6a0296d437
4 changed files with 62 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
Extras/EPA/Epa.cpp
View File

@@ -68,7 +68,7 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
while ( true ) while ( true )
{ {
assert( ( v.length2() > 0 ) && "Warning : v has zero magnitude!" ); EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Warning : v has zero magnitude!" );
SimdVector3 seperatingAxisInA = -v * m_transformA.getBasis(); SimdVector3 seperatingAxisInA = -v * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = v * m_transformB.getBasis(); SimdVector3 seperatingAxisInB = v * m_transformB.getBasis();
@@ -82,20 +82,20 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
SimdVector3 w = pWorld - qWorld; SimdVector3 w = pWorld - qWorld;
delta = v.dot( w ); delta = v.dot( w );
assert( ( delta <= 0 ) && "Shapes are disjoint, EPA should have never been called!" ); EPA_DEBUG_ASSERT( ( delta <= 0 ) ,"Shapes are disjoint, EPA should have never been called!" );
assert( !simplexSolver.inSimplex( w ) && "Shapes are disjoint, EPA should have never been called!" ); EPA_DEBUG_ASSERT( !simplexSolver.inSimplex( w ) ,"Shapes are disjoint, EPA should have never been called!" );
// Add support point to simplex // Add support point to simplex
simplexSolver.addVertex( w, pWorld, qWorld ); simplexSolver.addVertex( w, pWorld, qWorld );
bool closestOk = simplexSolver.closest( v ); bool closestOk = simplexSolver.closest( v );
assert( closestOk && "Shapes are disjoint, EPA should have never been called!" ); EPA_DEBUG_ASSERT( closestOk ,"Shapes are disjoint, EPA should have never been called!" );
SimdScalar prevVSqrd = squaredDistance; SimdScalar prevVSqrd = squaredDistance;
squaredDistance = v.length2(); squaredDistance = v.length2();
// Is v converging to v(A-B) ? // Is v converging to v(A-B) ?
assert( ( ( prevVSqrd - squaredDistance ) > SIMD_EPSILON * prevVSqrd ) && EPA_DEBUG_ASSERT( ( ( prevVSqrd - squaredDistance ) > SIMD_EPSILON * prevVSqrd ) ,
"Shapes are disjoint, EPA should have never been called!" ); "Shapes are disjoint, EPA should have never been called!" );
if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) ) if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
@@ -114,7 +114,7 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
// nbSimplexPoints can't be one because cases where the origin is on the boundary are handled // nbSimplexPoints can't be one because cases where the origin is on the boundary are handled
// by hybrid penetration depth // by hybrid penetration depth
assert( ( ( nbSimplexPoints > 1 ) && ( nbSimplexPoints <= 4 ) ) && EPA_DEBUG_ASSERT( ( ( nbSimplexPoints > 1 ) ,( nbSimplexPoints <= 4 ) ) ,
"Hybrid Penetration Depth algorithm failed!" ); "Hybrid Penetration Depth algorithm failed!" );
int nbPolyhedronPoints = nbSimplexPoints; int nbPolyhedronPoints = nbSimplexPoints;
@@ -302,7 +302,7 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
#ifdef _DEBUG #ifdef _DEBUG
else if ( nbSimplexPoints == 4 ) else if ( nbSimplexPoints == 4 )
{ {
assert( TetrahedronContainsOrigin( simplexPoints ) && "Initial tetrahedron does not contain the origin!" ); EPA_DEBUG_ASSERT( TetrahedronContainsOrigin( simplexPoints ) ,"Initial tetrahedron does not contain the origin!" );
} }
#endif #endif
@@ -330,7 +330,7 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
#endif #endif
{ {
// Failed to create initial polyhedron // Failed to create initial polyhedron
assert( false && "Failed to create initial polyhedron!" ); EPA_DEBUG_ASSERT( false ,"Failed to create initial polyhedron!" );
return false; return false;
} }
@@ -360,13 +360,13 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
if ( pFace->IsAffinelyDependent() ) if ( pFace->IsAffinelyDependent() )
{ {
assert( false && "One initial face is affinely dependent!" ); EPA_DEBUG_ASSERT( false ,"One initial face is affinely dependent!" );
return false; return false;
} }
if ( pFace->m_vSqrd <= 0 ) if ( pFace->m_vSqrd <= 0 )
{ {
assert( false && "Face containing the origin!" ); EPA_DEBUG_ASSERT( false ,"Face containing the origin!" );
return false; return false;
} }
@@ -384,7 +384,7 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
//m_polyhedron._dbgSaveToFile( "epa_start.dbg" ); //m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
#endif #endif
assert( !m_faceEntries.empty() && "No faces added to heap!" ); EPA_DEBUG_ASSERT( !m_faceEntries.empty() ,"No faces added to heap!" );
return true; return true;
} }
@@ -432,9 +432,9 @@ SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
#ifdef _DEBUG #ifdef _DEBUG
//assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" ); //assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" );
assert( ( vSqrd >= prevVSqrd ) && "vSqrd decreased!" ); EPA_DEBUG_ASSERT( ( vSqrd >= prevVSqrd ) ,"vSqrd decreased!" );
#endif //_DEBUG #endif //_DEBUG
assert( ( v.length2() > 0 ) && "Zero vector not allowed!" ); EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Zero vector not allowed!" );
SimdVector3 seperatingAxisInA = v * m_transformA.getBasis(); SimdVector3 seperatingAxisInA = v * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -v * m_transformB.getBasis(); SimdVector3 seperatingAxisInB = -v * m_transformB.getBasis();
@@ -461,7 +461,7 @@ SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
if ( expandOk ) if ( expandOk )
{ {
assert( !newFaces.empty() && "EPA polyhedron not expanding ?" ); EPA_DEBUG_ASSERT( !newFaces.empty() ,"EPA polyhedron not expanding ?" );
bool check = true; bool check = true;
bool areEqual = false; bool areEqual = false;
@@ -469,12 +469,12 @@ SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
while ( !newFaces.empty() ) while ( !newFaces.empty() )
{ {
EpaFace* pNewFace = newFaces.front(); EpaFace* pNewFace = newFaces.front();
assert( !pNewFace->m_deleted && "New face is deleted!" ); EPA_DEBUG_ASSERT( !pNewFace->m_deleted ,"New face is deleted!" );
if ( !pNewFace->m_deleted ) if ( !pNewFace->m_deleted )
{ {
assert( ( pNewFace->m_vSqrd > 0 ) && "Face containing the origin!" ); EPA_DEBUG_ASSERT( ( pNewFace->m_vSqrd > 0 ) ,"Face containing the origin!" );
assert( !pNewFace->IsAffinelyDependent() && "Face is affinely dependent!" ); EPA_DEBUG_ASSERT( !pNewFace->IsAffinelyDependent() ,"Face is affinely dependent!" );
//#ifdef EPA_POLYHEDRON_USE_PLANES //#ifdef EPA_POLYHEDRON_USE_PLANES
//// if ( pNewFace->m_planeDistance >= 0 ) //// if ( pNewFace->m_planeDistance >= 0 )
@@ -527,7 +527,7 @@ SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
//m_polyhedron._dbgSaveToFile( "epa_end.dbg" ); //m_polyhedron._dbgSaveToFile( "epa_end.dbg" );
#endif #endif
assert( pEpaFace && "Invalid epa face!" ); EPA_DEBUG_ASSERT( pEpaFace ,"Invalid epa face!" );
pEpaFace->CalcClosestPointOnA( wWitnessOnA ); pEpaFace->CalcClosestPointOnA( wWitnessOnA );
pEpaFace->CalcClosestPointOnB( wWitnessOnB ); pEpaFace->CalcClosestPointOnB( wWitnessOnB );

2
Extras/EPA/Epa.h
View File

@@ -19,6 +19,8 @@ subject to the following restrictions:
#define EPA_MAX_FACE_ENTRIES 256 #define EPA_MAX_FACE_ENTRIES 256
extern const SimdScalar EPA_MAX_RELATIVE_ERROR; extern const SimdScalar EPA_MAX_RELATIVE_ERROR;
extern const SimdScalar EPA_MAX_RELATIVE_ERROR_SQRD; extern const SimdScalar EPA_MAX_RELATIVE_ERROR_SQRD;

5
Extras/EPA/EpaCommon.h
View File

@@ -19,6 +19,11 @@ subject to the following restrictions:
#define EPA_POLYHEDRON_USE_PLANES #define EPA_POLYHEDRON_USE_PLANES
//#define EPA_DEBUG_ASSERT(c,a)
//#define EPA_DEBUG_ASSERT(c,a) assert(c && a)
#define EPA_DEBUG_ASSERT(c,a) if (!c) printf(a)
//#define EPA_USE_HYBRID //#define EPA_USE_HYBRID
#endif #endif

62
Extras/EPA/EpaPolyhedron.cpp
View File

@@ -47,7 +47,7 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
const int nbInitialPoints ) const int nbInitialPoints )
{ {
#ifndef EPA_POLYHEDRON_USE_PLANES #ifndef EPA_POLYHEDRON_USE_PLANES
assert( ( nbInitialPoints <= 4 ) && "nbInitialPoints greater than 4!" ); EPA_DEBUG_ASSERT( ( nbInitialPoints <= 4 ) ,"nbInitialPoints greater than 4!" );
#endif #endif
if ( nbInitialPoints < 4 ) if ( nbInitialPoints < 4 )
@@ -184,7 +184,7 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
if ( nbSuccessfullAxis <= 1 ) if ( nbSuccessfullAxis <= 1 )
{ {
// Degenerate input ? // Degenerate input ?
assert( false && "nbSuccessfullAxis must be greater than 1!" ); EPA_DEBUG_ASSERT( false ,"nbSuccessfullAxis must be greater than 1!" );
return false; return false;
} }
@@ -211,12 +211,24 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
#endif #endif
#ifdef EPA_POLYHEDRON_USE_PLANES #ifdef EPA_POLYHEDRON_USE_PLANES
assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 0 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) && bool pointOnPlane0 = SimdEqual( pInitialPoints[ finalPointsIndices[ 0 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
"Point should be on plane!" ); if (!pointOnPlane0)
assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 1 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) && {
"Point should be on plane!" ); EPA_DEBUG_ASSERT(0,"Point0 should be on plane!");
assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 2 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) && return false;
"Point should be on plane!" ); }
bool pointOnPlane1 = SimdEqual( pInitialPoints[ finalPointsIndices[ 1 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane1)
{
EPA_DEBUG_ASSERT(0,"Point1 should be on plane!");
return false;
}
bool pointOnPlane2 = SimdEqual( pInitialPoints[ finalPointsIndices[ 2 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane2)
{
EPA_DEBUG_ASSERT(0,"Point2 should be on plane!");
return false;
}
#endif #endif
#ifndef EPA_POLYHEDRON_USE_PLANES #ifndef EPA_POLYHEDRON_USE_PLANES
@@ -395,7 +407,7 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
if ( !pFaceA->Initialize() || !pFaceB->Initialize() || if ( !pFaceA->Initialize() || !pFaceB->Initialize() ||
!pFaceC->Initialize() || !pFaceD->Initialize() ) !pFaceC->Initialize() || !pFaceD->Initialize() )
{ {
assert( false && "One initial face failed to initialize!" ); EPA_DEBUG_ASSERT( false, "One initial face failed to initialize!" );
return false; return false;
} }
@@ -428,7 +440,7 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
return false; return false;
} }
assert( !newFaces.empty() && "Polyhedron should have expanded!" ); EPA_DEBUG_ASSERT( !newFaces.empty() ,"Polyhedron should have expanded!" );
break; break;
} }
@@ -455,7 +467,7 @@ void EpaPolyhedron::Destroy()
EpaFace* EpaPolyhedron::CreateFace() EpaFace* EpaPolyhedron::CreateFace()
{ {
EpaFace* pNewFace = new EpaFace(); EpaFace* pNewFace = new EpaFace();
assert( pNewFace && "Failed to allocate memory for a new EpaFace!" ); EPA_DEBUG_ASSERT( pNewFace , "Failed to allocate memory for a new EpaFace!" );
m_faces.push_back( pNewFace ); m_faces.push_back( pNewFace );
@@ -467,7 +479,7 @@ EpaFace* EpaPolyhedron::CreateFace()
EpaHalfEdge* EpaPolyhedron::CreateHalfEdge() EpaHalfEdge* EpaPolyhedron::CreateHalfEdge()
{ {
EpaHalfEdge* pNewHalfEdge = new EpaHalfEdge(); EpaHalfEdge* pNewHalfEdge = new EpaHalfEdge();
assert( pNewHalfEdge && "Failed to allocate memory for a new EpaHalfEdge!" ); EPA_DEBUG_ASSERT( pNewHalfEdge ,"Failed to allocate memory for a new EpaHalfEdge!" );
m_halfEdges.push_back( pNewHalfEdge ); m_halfEdges.push_back( pNewHalfEdge );
@@ -479,7 +491,7 @@ EpaVertex* EpaPolyhedron::CreateVertex( const SimdPoint3& wSupportPoint,
const SimdPoint3& wSupportPointOnB ) const SimdPoint3& wSupportPointOnB )
{ {
EpaVertex* pNewVertex = new EpaVertex( wSupportPoint, wSupportPointOnA, wSupportPointOnB ); EpaVertex* pNewVertex = new EpaVertex( wSupportPoint, wSupportPointOnA, wSupportPointOnB );
assert( pNewVertex && "Failed to allocate memory for a new EpaVertex!" ); EPA_DEBUG_ASSERT( pNewVertex ,"Failed to allocate memory for a new EpaVertex!" );
m_vertices.push_back( pNewVertex ); m_vertices.push_back( pNewVertex );
@@ -535,10 +547,10 @@ bool EpaPolyhedron::Expand( const SimdPoint3& wSupportPoint,
const SimdPoint3& wSupportPointOnB, const SimdPoint3& wSupportPointOnB,
EpaFace* pFace, std::list< EpaFace* >& newFaces ) EpaFace* pFace, std::list< EpaFace* >& newFaces )
{ {
assert( !pFace->m_deleted && "Face is already deleted!" ); EPA_DEBUG_ASSERT( !pFace->m_deleted ,"Face is already deleted!" );
assert( newFaces.empty() && "NewFaces list must be empty!" ); EPA_DEBUG_ASSERT( newFaces.empty() ,"NewFaces list must be empty!" );
assert( !pFace->m_deleted && "Cannot expand deleted face!" ); EPA_DEBUG_ASSERT( !pFace->m_deleted ,"Cannot expand deleted face!" );
// wSupportPoint must be front of face's plane used to do the expansion // wSupportPoint must be front of face's plane used to do the expansion
@@ -553,10 +565,10 @@ bool EpaPolyhedron::Expand( const SimdPoint3& wSupportPoint,
std::list< EpaHalfEdge* > coneBaseEdges; std::list< EpaHalfEdge* > coneBaseEdges;
DeleteVisibleFaces( wSupportPoint, pFace, coneBaseEdges ); DeleteVisibleFaces( wSupportPoint, pFace, coneBaseEdges );
assert( ( coneBaseEdges.size() >= 3 ) && "Cone base must have at least 3 edges!" ); EPA_DEBUG_ASSERT( ( coneBaseEdges.size() >= 3 ) ,"Cone base must have at least 3 edges!" );
EpaVertex* pConeAppex = CreateVertex( wSupportPoint, wSupportPointOnA, wSupportPointOnB ); EpaVertex* pConeAppex = CreateVertex( wSupportPoint, wSupportPointOnA, wSupportPointOnB );
assert( pConeAppex && "Failed to create vertex!" ); EPA_DEBUG_ASSERT( pConeAppex ,"Failed to create vertex!" );
CreateCone( pConeAppex, coneBaseEdges, newFaces ); CreateCone( pConeAppex, coneBaseEdges, newFaces );
@@ -592,7 +604,7 @@ int EpaPolyhedron::GetNbFaces() const
void EpaPolyhedron::DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace, void EpaPolyhedron::DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace,
std::list< EpaHalfEdge* >& coneBaseTwinHalfEdges ) std::list< EpaHalfEdge* >& coneBaseTwinHalfEdges )
{ {
assert( !pFace->m_deleted && "Face is already deleted!" ); EPA_DEBUG_ASSERT( !pFace->m_deleted ,"Face is already deleted!" );
DeleteFace( pFace ); DeleteFace( pFace );
@@ -600,15 +612,15 @@ void EpaPolyhedron::DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace,
do do
{ {
assert( pCurrentHalfEdge->m_pTwin && "Half-edge without a twin!" ); EPA_DEBUG_ASSERT( pCurrentHalfEdge->m_pTwin ,"Half-edge without a twin!" );
EpaFace* pAdjacentFace = pCurrentHalfEdge->m_pTwin->m_pFace; EpaFace* pAdjacentFace = pCurrentHalfEdge->m_pTwin->m_pFace;
assert( pAdjacentFace && "Invalid adjacent face!" ); EPA_DEBUG_ASSERT( pAdjacentFace ,"Invalid adjacent face!" );
if ( !pAdjacentFace->m_deleted ) if ( !pAdjacentFace->m_deleted )
{ {
#ifdef EPA_POLYHEDRON_USE_PLANES #ifdef EPA_POLYHEDRON_USE_PLANES
assert( ( pAdjacentFace->m_planeNormal.length2() > 0 ) && "Invalid plane!" ); EPA_DEBUG_ASSERT( ( pAdjacentFace->m_planeNormal.length2() > 0 ) ,"Invalid plane!" );
SimdScalar pointDist = pAdjacentFace->m_planeNormal.dot( point ) + SimdScalar pointDist = pAdjacentFace->m_planeNormal.dot( point ) +
pAdjacentFace->m_planeDistance; pAdjacentFace->m_planeDistance;
@@ -635,7 +647,7 @@ void EpaPolyhedron::DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace,
void EpaPolyhedron::CreateCone( EpaVertex* pAppexVertex, std::list< EpaHalfEdge* >& baseTwinHalfEdges, void EpaPolyhedron::CreateCone( EpaVertex* pAppexVertex, std::list< EpaHalfEdge* >& baseTwinHalfEdges,
std::list< EpaFace* >& newFaces ) std::list< EpaFace* >& newFaces )
{ {
assert( ( baseTwinHalfEdges.size() >= 3 ) && "DeleteVisibleFaces method didn't do its job right!" ); EPA_DEBUG_ASSERT( ( baseTwinHalfEdges.size() >= 3 ) ,"DeleteVisibleFaces method didn't do its job right!" );
std::list< EpaHalfEdge* >::iterator baseHalfEdgesItr( baseTwinHalfEdges.begin() ); std::list< EpaHalfEdge* >::iterator baseHalfEdgesItr( baseTwinHalfEdges.begin() );
std::list< EpaHalfEdge* > halfEdgesToLink; std::list< EpaHalfEdge* > halfEdgesToLink;
@@ -651,7 +663,7 @@ void EpaPolyhedron::CreateCone( EpaVertex* pAppexVertex, std::list< EpaHalfEdge*
// Connect consecutive faces by linking twin half-edges // Connect consecutive faces by linking twin half-edges
assert( ( halfEdgesToLink.size() % 2 == 0 ) && "Nb half-edges to link is odd!" ); EPA_DEBUG_ASSERT( ( halfEdgesToLink.size() % 2 == 0 ) ,"Nb half-edges to link is odd!" );
int nbLinksToCreate = halfEdgesToLink.size() / 2; int nbLinksToCreate = halfEdgesToLink.size() / 2;
int nbLinksCreated = 0; int nbLinksCreated = 0;
@@ -682,7 +694,7 @@ void EpaPolyhedron::CreateCone( EpaVertex* pAppexVertex, std::list< EpaHalfEdge*
} }
} }
assert( ( nbLinksCreated == nbLinksToCreate ) && "Mesh topology not ok!" ); EPA_DEBUG_ASSERT( ( nbLinksCreated == nbLinksToCreate ) ,"Mesh topology not ok!" );
} }
EpaFace* EpaPolyhedron::CreateConeFace( EpaVertex* pAppexVertex, EpaHalfEdge* pBaseTwinHalfEdge, EpaFace* EpaPolyhedron::CreateConeFace( EpaVertex* pAppexVertex, EpaHalfEdge* pBaseTwinHalfEdge,