AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-19 05:20:06 +00:00
Code Issues 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
Show Stats 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 )
{
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 seperatingAxisInB = v * m_transformB.getBasis();
@ -82,20 +82,20 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
SimdVector3 w = pWorld - qWorld;
delta = v.dot( w );
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( ( delta <= 0 ) ,"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
simplexSolver.addVertex( w, pWorld, qWorld );
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;
squaredDistance = v.length2();
// 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!" );
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
// by hybrid penetration depth
assert( ( ( nbSimplexPoints > 1 ) && ( nbSimplexPoints <= 4 ) ) &&
EPA_DEBUG_ASSERT( ( ( nbSimplexPoints > 1 ) ,( nbSimplexPoints <= 4 ) ) ,
"Hybrid Penetration Depth algorithm failed!" );
int nbPolyhedronPoints = nbSimplexPoints;
@ -302,7 +302,7 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
#ifdef _DEBUG
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
@ -330,7 +330,7 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
#endif
{
// Failed to create initial polyhedron
assert( false && "Failed to create initial polyhedron!" );
EPA_DEBUG_ASSERT( false ,"Failed to create initial polyhedron!" );
return false;
}
@ -360,13 +360,13 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
if ( pFace->IsAffinelyDependent() )
{
assert( false && "One initial face is affinely dependent!" );
EPA_DEBUG_ASSERT( false ,"One initial face is affinely dependent!" );
return false;
}
if ( pFace->m_vSqrd <= 0 )
{
assert( false && "Face containing the origin!" );
EPA_DEBUG_ASSERT( false ,"Face containing the origin!" );
return false;
}
@ -384,7 +384,7 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
//m_polyhedron._dbgSaveToFile( "epa_start.dbg" );
#endif
assert( !m_faceEntries.empty() && "No faces added to heap!" );
EPA_DEBUG_ASSERT( !m_faceEntries.empty() ,"No faces added to heap!" );
return true;
}
@ -432,9 +432,9 @@ SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
#ifdef _DEBUG
//assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" );
assert( ( vSqrd >= prevVSqrd ) && "vSqrd decreased!" );
EPA_DEBUG_ASSERT( ( vSqrd >= prevVSqrd ) ,"vSqrd decreased!" );
#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 seperatingAxisInB = -v * m_transformB.getBasis();
@ -461,7 +461,7 @@ SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
if ( expandOk )
{
assert( !newFaces.empty() && "EPA polyhedron not expanding ?" );
EPA_DEBUG_ASSERT( !newFaces.empty() ,"EPA polyhedron not expanding ?" );
bool check = true;
bool areEqual = false;
@ -469,12 +469,12 @@ SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
while ( !newFaces.empty() )
{
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 )
{
assert( ( pNewFace->m_vSqrd > 0 ) && "Face containing the origin!" );
assert( !pNewFace->IsAffinelyDependent() && "Face is affinely dependent!" );
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 )
@ -527,7 +527,7 @@ SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
//m_polyhedron._dbgSaveToFile( "epa_end.dbg" );
#endif
assert( pEpaFace && "Invalid epa face!" );
EPA_DEBUG_ASSERT( pEpaFace ,"Invalid epa face!" );
pEpaFace->CalcClosestPointOnA( wWitnessOnA );
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
extern const SimdScalar EPA_MAX_RELATIVE_ERROR;
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_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
#endif

62
Extras/EPA/EpaPolyhedron.cpp
View File

@ -47,7 +47,7 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
const int nbInitialPoints )
{
#ifndef EPA_POLYHEDRON_USE_PLANES
assert( ( nbInitialPoints <= 4 ) && "nbInitialPoints greater than 4!" );
EPA_DEBUG_ASSERT( ( nbInitialPoints <= 4 ) ,"nbInitialPoints greater than 4!" );
#endif
if ( nbInitialPoints < 4 )
@ -184,7 +184,7 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
if ( nbSuccessfullAxis <= 1 )
{
// Degenerate input ?
assert( false && "nbSuccessfullAxis must be greater than 1!" );
EPA_DEBUG_ASSERT( false ,"nbSuccessfullAxis must be greater than 1!" );
return false;
}
@ -211,12 +211,24 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
#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!" );
bool pointOnPlane0 = SimdEqual( pInitialPoints[ finalPointsIndices[ 0 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane0)
{
EPA_DEBUG_ASSERT(0,"Point0 should be on plane!");
return false;
}
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
#ifndef EPA_POLYHEDRON_USE_PLANES
@ -395,7 +407,7 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
if ( !pFaceA->Initialize() || !pFaceB->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;
}
@ -428,7 +440,7 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
return false;
}
assert( !newFaces.empty() && "Polyhedron should have expanded!" );
EPA_DEBUG_ASSERT( !newFaces.empty() ,"Polyhedron should have expanded!" );
break;
}
@ -455,7 +467,7 @@ void EpaPolyhedron::Destroy()
EpaFace* EpaPolyhedron::CreateFace()
{
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 );
@ -467,7 +479,7 @@ EpaFace* EpaPolyhedron::CreateFace()
EpaHalfEdge* EpaPolyhedron::CreateHalfEdge()
{
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 );
@ -479,7 +491,7 @@ EpaVertex* EpaPolyhedron::CreateVertex( const SimdPoint3& wSupportPoint,
const SimdPoint3& 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 );
@ -535,10 +547,10 @@ bool EpaPolyhedron::Expand( const SimdPoint3& wSupportPoint,
const SimdPoint3& wSupportPointOnB,
EpaFace* pFace, std::list< EpaFace* >& newFaces )
{
assert( !pFace->m_deleted && "Face is already deleted!" );
assert( newFaces.empty() && "NewFaces list must be empty!" );
EPA_DEBUG_ASSERT( !pFace->m_deleted ,"Face is already deleted!" );
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
@ -553,10 +565,10 @@ bool EpaPolyhedron::Expand( const SimdPoint3& wSupportPoint,
std::list< EpaHalfEdge* > 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 );
assert( pConeAppex && "Failed to create vertex!" );
EPA_DEBUG_ASSERT( pConeAppex ,"Failed to create vertex!" );
CreateCone( pConeAppex, coneBaseEdges, newFaces );
@ -592,7 +604,7 @@ int EpaPolyhedron::GetNbFaces() const
void EpaPolyhedron::DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace,
std::list< EpaHalfEdge* >& coneBaseTwinHalfEdges )
{
assert( !pFace->m_deleted && "Face is already deleted!" );
EPA_DEBUG_ASSERT( !pFace->m_deleted ,"Face is already deleted!" );
DeleteFace( pFace );
@ -600,15 +612,15 @@ void EpaPolyhedron::DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace,
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;
assert( pAdjacentFace && "Invalid adjacent face!" );
EPA_DEBUG_ASSERT( pAdjacentFace ,"Invalid adjacent face!" );
if ( !pAdjacentFace->m_deleted )
{
#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 ) +
pAdjacentFace->m_planeDistance;
@ -635,7 +647,7 @@ void EpaPolyhedron::DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace,
void EpaPolyhedron::CreateCone( EpaVertex* pAppexVertex, std::list< EpaHalfEdge* >& baseTwinHalfEdges,
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* > halfEdgesToLink;
@ -651,7 +663,7 @@ void EpaPolyhedron::CreateCone( EpaVertex* pAppexVertex, std::list< EpaHalfEdge*
// 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 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,