570 lines
17 KiB
C++
570 lines
17 KiB
C++
/*
|
|
Bullet Continuous Collision Detection and Physics Library
|
|
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
|
|
|
EPA Copyright (c) Ricardo Padrela 2006
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
#include "LinearMath/btScalar.h"
|
|
#include "LinearMath/btVector3.h"
|
|
#include "LinearMath/btPoint3.h"
|
|
#include "LinearMath/btTransform.h"
|
|
#include "LinearMath/btSimdMinMax.h"
|
|
|
|
#include "BulletCollision/CollisionShapes/btConvexShape.h"
|
|
|
|
#include <vector>
|
|
#include <list>
|
|
#include <algorithm>
|
|
|
|
#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
|
|
|
|
#include "NarrowPhaseCollision/EpaCommon.h"
|
|
|
|
#include "NarrowPhaseCollision/EpaVertex.h"
|
|
#include "NarrowPhaseCollision/EpaHalfEdge.h"
|
|
#include "NarrowPhaseCollision/EpaFace.h"
|
|
#include "NarrowPhaseCollision/EpaPolyhedron.h"
|
|
#include "NarrowPhaseCollision/Epa.h"
|
|
|
|
const btScalar EPA_MAX_RELATIVE_ERROR = 1e-2f;
|
|
const btScalar EPA_MAX_RELATIVE_ERROR_SQRD = EPA_MAX_RELATIVE_ERROR * EPA_MAX_RELATIVE_ERROR;
|
|
|
|
Epa::Epa( btConvexShape* pConvexShapeA, btConvexShape* pConvexShapeB,
|
|
const btTransform& transformA, const btTransform& transformB ) : m_pConvexShapeA( pConvexShapeA ),
|
|
m_pConvexShapeB( pConvexShapeB ),
|
|
m_transformA( transformA ),
|
|
m_transformB( transformB )
|
|
{
|
|
m_faceEntries.reserve( EPA_MAX_FACE_ENTRIES );
|
|
}
|
|
|
|
Epa::~Epa()
|
|
{
|
|
}
|
|
|
|
bool Epa::Initialize( btSimplexSolverInterface& simplexSolver )
|
|
{
|
|
// Run GJK on the enlarged shapes to obtain a simplex of the enlarged CSO
|
|
|
|
btVector3 v( 1, 0, 0 );
|
|
btScalar squaredDistance = SIMD_INFINITY;
|
|
|
|
btScalar delta = 0.f;
|
|
|
|
simplexSolver.reset();
|
|
|
|
int nbIterations = 0;
|
|
|
|
while ( true )
|
|
{
|
|
EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Warning : v has zero magnitude!" );
|
|
|
|
btVector3 seperatingAxisInA = -v * m_transformA.getBasis();
|
|
btVector3 seperatingAxisInB = v * m_transformB.getBasis();
|
|
|
|
btVector3 pInA = m_pConvexShapeA->localGetSupportingVertex( seperatingAxisInA );
|
|
btVector3 qInB = m_pConvexShapeB->localGetSupportingVertex( seperatingAxisInB );
|
|
|
|
btPoint3 pWorld = m_transformA( pInA );
|
|
btPoint3 qWorld = m_transformB( qInB );
|
|
|
|
btVector3 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;
|
|
|
|
btScalar 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;
|
|
}
|
|
|
|
btPoint3 simplexPoints[ 5 ];
|
|
btPoint3 wSupportPointsOnA[ 5 ];
|
|
btPoint3 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
|
|
|
|
btVector3 d = simplexPoints[ 0 ] - simplexPoints[ 1 ];
|
|
d.normalize();
|
|
|
|
btVector3 v1;
|
|
btVector3 v2;
|
|
btVector3 v3;
|
|
|
|
btVector3 e1;
|
|
|
|
btScalar absx = abs( d.getX() );
|
|
btScalar absy = abs( d.getY() );
|
|
btScalar 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;
|
|
|
|
btVector3 seperatingAxisInA = v1 * m_transformA.getBasis();
|
|
btVector3 seperatingAxisInB = -v1 * m_transformB.getBasis();
|
|
|
|
btVector3 p = m_pConvexShapeA->localGetSupportingVertex( seperatingAxisInA );
|
|
btVector3 q = m_pConvexShapeB->localGetSupportingVertex( seperatingAxisInB );
|
|
|
|
btPoint3 pWorld = m_transformA( p );
|
|
btPoint3 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
|
|
|
|
btVector3 v0 = simplexPoints[ 2 ] - simplexPoints[ 0 ];
|
|
btVector3 v1 = simplexPoints[ 1 ] - simplexPoints[ 0 ];
|
|
btVector3 triangleNormal = v0.cross( v1 );
|
|
triangleNormal.normalize();
|
|
|
|
nbPolyhedronPoints = 5;
|
|
|
|
btVector3 seperatingAxisInA = triangleNormal * m_transformA.getBasis();
|
|
btVector3 seperatingAxisInB = -triangleNormal * m_transformB.getBasis();
|
|
|
|
btVector3 p = m_pConvexShapeA->localGetSupportingVertex( seperatingAxisInA );
|
|
btVector3 q = m_pConvexShapeB->localGetSupportingVertex( seperatingAxisInB );
|
|
|
|
btPoint3 pWorld = m_transformA( p );
|
|
btPoint3 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
|
|
btPoint3 wTetraPoints[ 4 ] = { simplexPoints[ initTetraIndices[ 0 ] ],
|
|
simplexPoints[ initTetraIndices[ 1 ] ],
|
|
simplexPoints[ initTetraIndices[ 2 ] ],
|
|
simplexPoints[ initTetraIndices[ 3 ] ] };
|
|
|
|
btPoint3 wTetraSupportPointsOnA[ 4 ] = { wSupportPointsOnA[ initTetraIndices[ 0 ] ],
|
|
wSupportPointsOnA[ initTetraIndices[ 1 ] ],
|
|
wSupportPointsOnA[ initTetraIndices[ 2 ] ],
|
|
wSupportPointsOnA[ initTetraIndices[ 3 ] ] };
|
|
|
|
btPoint3 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;
|
|
}
|
|
|
|
btScalar Epa::calcPenDepth( btPoint3& wWitnessOnA, btPoint3& wWitnessOnB )
|
|
{
|
|
btVector3 v;
|
|
|
|
btScalar upperBoundSqrd = SIMD_INFINITY;
|
|
btScalar vSqrd = 0;
|
|
#ifdef _DEBUG
|
|
btScalar prevVSqrd;
|
|
#endif
|
|
btScalar 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!" );
|
|
|
|
btVector3 seperatingAxisInA = v * m_transformA.getBasis();
|
|
btVector3 seperatingAxisInB = -v * m_transformB.getBasis();
|
|
|
|
btVector3 p = m_pConvexShapeA->localGetSupportingVertex( seperatingAxisInA );
|
|
btVector3 q = m_pConvexShapeB->localGetSupportingVertex( seperatingAxisInB );
|
|
|
|
btPoint3 pWorld = m_transformA( p );
|
|
btPoint3 qWorld = m_transformB( q );
|
|
|
|
btPoint3 w = pWorld - qWorld;
|
|
delta = v.dot( w );
|
|
|
|
// Keep tighest upper bound
|
|
upperBoundSqrd = btMin( 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 Epa::TetrahedronContainsOrigin( const btPoint3& point0, const btPoint3& point1,
|
|
const btPoint3& point2, const btPoint3& point3 )
|
|
{
|
|
btVector3 facesNormals[ 4 ] = { ( point1 - point0 ).cross( point2 - point0 ),
|
|
( point2 - point1 ).cross( point3 - point1 ),
|
|
( point3 - point2 ).cross( point0 - point2 ),
|
|
( point0 - point3 ).cross( point1 - point3 ) };
|
|
|
|
return ( ( facesNormals[ 0 ].dot( point0 ) > 0 ) != ( facesNormals[ 0 ].dot( point3 ) > 0 ) ) &&
|
|
( ( facesNormals[ 1 ].dot( point1 ) > 0 ) != ( facesNormals[ 1 ].dot( point0 ) > 0 ) ) &&
|
|
( ( facesNormals[ 2 ].dot( point2 ) > 0 ) != ( facesNormals[ 2 ].dot( point1 ) > 0 ) ) &&
|
|
( ( facesNormals[ 3 ].dot( point3 ) > 0 ) != ( facesNormals[ 3 ].dot( point2 ) > 0 ) );
|
|
}
|
|
|
|
bool Epa::TetrahedronContainsOrigin( btPoint3* pPoints )
|
|
{
|
|
return TetrahedronContainsOrigin( pPoints[ 0 ], pPoints[ 1 ], pPoints[ 2 ], pPoints[ 3 ] );
|
|
}
|
|
|
|
bool CompareEpaFaceEntries( EpaFace* pFaceA, EpaFace* pFaceB )
|
|
{
|
|
return ( pFaceA->m_vSqrd > pFaceB->m_vSqrd );
|
|
}
|