pass on rigid body name in btBulletWorldImporter, to make it easier to bind physics and graphics objects.
moved some obsolete files to Extras/obsolete, and removed freeglut moved ColladaDemo to Dynamica Maya plugin repository (it has COLLADA_DOM and libxml), see http://dynamica.googlecode.com Added new .bullet file Minor update in Bullet_User_Manual.pdf, removed obsolete Bullet_Faq.pdf
This commit is contained in:
erwin.coumans
569
Extras/obsolete/EPA/Epa.cpp
Normal file
569
Extras/obsolete/EPA/Epa.cpp
Normal file
@@ -0,0 +1,569 @@
|
||||
/*
|
||||
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/btMinMax.h"
|
||||
|
||||
#include "BulletCollision/CollisionShapes/btConvexShape.h"
|
||||
|
||||
#include <vector>
|
||||
#include <list>
|
||||
#include <algorithm>
|
||||
|
||||
#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
|
||||
|
||||
#include "EpaCommon.h"
|
||||
|
||||
#include "EpaVertex.h"
|
||||
#include "EpaHalfEdge.h"
|
||||
#include "EpaFace.h"
|
||||
#include "EpaPolyhedron.h"
|
||||
#include "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 );
|
||||
}
|
||||
Reference in New Issue
Block a user