Refactoring:
Moved optional code to Extras: AlgebraicCCD,EPA,quickstep Moved SimpleBroadphase data to OverlappingPairCache, and derive both SimpleBroadphase and AxisSweep3 from OverlappingPairCache. Added ParallelPhysicsEnvironment (prepair more parallel mainloop) Upgraded hardcoded limit from 1024/8192 to 32766/65535 (max objects / max overlapping pairs)
This commit is contained in:
ejcoumans
560
Extras/EPA/Epa.cpp
Normal file
560
Extras/EPA/Epa.cpp
Normal file
@@ -0,0 +1,560 @@
|
||||
/*
|
||||
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 "SimdScalar.h"
|
||||
#include "SimdVector3.h"
|
||||
#include "SimdPoint3.h"
|
||||
#include "SimdTransform.h"
|
||||
#include "SimdMinMax.h"
|
||||
|
||||
#include "CollisionShapes/ConvexShape.h"
|
||||
|
||||
#include <vector>
|
||||
#include <list>
|
||||
#include <algorithm>
|
||||
|
||||
#include "NarrowPhaseCollision/SimplexSolverInterface.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 SimdScalar EPA_MAX_RELATIVE_ERROR = 1e-2f;
|
||||
const SimdScalar EPA_MAX_RELATIVE_ERROR_SQRD = EPA_MAX_RELATIVE_ERROR * EPA_MAX_RELATIVE_ERROR;
|
||||
|
||||
Epa::Epa( ConvexShape* pConvexShapeA, ConvexShape* pConvexShapeB,
|
||||
const SimdTransform& transformA, const SimdTransform& 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( 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 )
|
||||
{
|
||||
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 );
|
||||
|
||||
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!" );
|
||||
|
||||
// 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!" );
|
||||
|
||||
SimdScalar prevVSqrd = squaredDistance;
|
||||
squaredDistance = v.length2();
|
||||
|
||||
// Is v converging to v(A-B) ?
|
||||
assert( ( ( prevVSqrd - squaredDistance ) > SIMD_EPSILON * prevVSqrd ) &&
|
||||
"Shapes are disjoint, EPA should have never been called!" );
|
||||
|
||||
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
|
||||
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 )
|
||||
{
|
||||
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
|
||||
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() )
|
||||
{
|
||||
assert( false && "One initial face is affinely dependent!" );
|
||||
return false;
|
||||
}
|
||||
|
||||
if ( pFace->m_vSqrd <= 0 )
|
||||
{
|
||||
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
|
||||
|
||||
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!" );
|
||||
assert( ( vSqrd >= prevVSqrd ) && "vSqrd decreased!" );
|
||||
#endif //_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 )
|
||||
{
|
||||
assert( !newFaces.empty() && "EPA polyhedron not expanding ?" );
|
||||
|
||||
bool check = true;
|
||||
bool areEqual = false;
|
||||
|
||||
while ( !newFaces.empty() )
|
||||
{
|
||||
EpaFace* pNewFace = newFaces.front();
|
||||
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!" );
|
||||
|
||||
//#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
|
||||
|
||||
assert( pEpaFace && "Invalid epa face!" );
|
||||
|
||||
pEpaFace->CalcClosestPointOnA( wWitnessOnA );
|
||||
pEpaFace->CalcClosestPointOnB( wWitnessOnB );
|
||||
|
||||
return v.length();
|
||||
}
|
||||
|
||||
bool Epa::TetrahedronContainsOrigin( const SimdPoint3& point0, const SimdPoint3& point1,
|
||||
const SimdPoint3& point2, const SimdPoint3& point3 )
|
||||
{
|
||||
SimdVector3 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( SimdPoint3* pPoints )
|
||||
{
|
||||
return TetrahedronContainsOrigin( pPoints[ 0 ], pPoints[ 1 ], pPoints[ 2 ], pPoints[ 3 ] );
|
||||
}
|
||||
|
||||
bool CompareEpaFaceEntries( EpaFace* pFaceA, EpaFace* pFaceB )
|
||||
{
|
||||
return ( pFaceA->m_vSqrd > pFaceB->m_vSqrd );
|
||||
}
|
||||
Reference in New Issue
Block a user