203 lines
7.2 KiB
C++
203 lines
7.2 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 <list>
|
|
|
|
#include "BulletCollision/CollisionShapes/btConvexShape.h"
|
|
|
|
#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"
|
|
#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
|
|
#include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h"
|
|
|
|
btScalar g_GJKMaxRelError = 1e-3f;
|
|
btScalar g_GJKMaxRelErrorSqrd = g_GJKMaxRelError * g_GJKMaxRelError;
|
|
|
|
bool EpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver,
|
|
btConvexShape* pConvexA, btConvexShape* pConvexB,
|
|
const btTransform& transformA, const btTransform& transformB,
|
|
btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
|
|
class btIDebugDraw* debugDraw )
|
|
{
|
|
EPA_DEBUG_ASSERT( pConvexA ,"Convex shape A is invalid!" );
|
|
EPA_DEBUG_ASSERT( pConvexB ,"Convex shape B is invalid!" );
|
|
|
|
btScalar penDepth;
|
|
|
|
#ifdef EPA_USE_HYBRID
|
|
bool needsEPA = !HybridPenDepth( simplexSolver, pConvexA, pConvexB, transformA, transformB,
|
|
wWitnessOnA, wWitnessOnB, penDepth, v );
|
|
|
|
if ( needsEPA )
|
|
{
|
|
#endif
|
|
penDepth = EpaPenDepth( simplexSolver, pConvexA, pConvexB,
|
|
transformA, transformB,
|
|
wWitnessOnA, wWitnessOnB );
|
|
EPA_DEBUG_ASSERT( ( penDepth > 0 ) ,"EPA or Hybrid Technique failed to calculate penetration depth!" );
|
|
|
|
#ifdef EPA_USE_HYBRID
|
|
}
|
|
#endif
|
|
|
|
return ( penDepth > 0 );
|
|
}
|
|
|
|
#ifdef EPA_USE_HYBRID
|
|
bool EpaPenetrationDepthSolver::HybridPenDepth( btSimplexSolverInterface& simplexSolver,
|
|
btConvexShape* pConvexA, btConvexShape* pConvexB,
|
|
const btTransform& transformA, const btTransform& transformB,
|
|
btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
|
|
btScalar& penDepth, btVector3& v )
|
|
{
|
|
btScalar squaredDistance = SIMD_INFINITY;
|
|
btScalar delta = 0.f;
|
|
|
|
const btScalar margin = pConvexA->getMargin() + pConvexB->getMargin();
|
|
const btScalar marginSqrd = margin * margin;
|
|
|
|
simplexSolver.reset();
|
|
|
|
int nbIterations = 0;
|
|
|
|
while ( true )
|
|
{
|
|
assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" );
|
|
|
|
btVector3 seperatingAxisInA = -v * transformA.getBasis();
|
|
btVector3 seperatingAxisInB = v * transformB.getBasis();
|
|
|
|
btVector3 pInA = pConvexA->localGetSupportingVertexWithoutMargin( seperatingAxisInA );
|
|
btVector3 qInB = pConvexB->localGetSupportingVertexWithoutMargin( seperatingAxisInB );
|
|
|
|
btPoint3 pWorld = transformA( pInA );
|
|
btPoint3 qWorld = transformB( qInB );
|
|
|
|
btVector3 w = pWorld - qWorld;
|
|
delta = v.dot( w );
|
|
|
|
// potential exit, they don't overlap
|
|
if ( ( delta > 0 ) && ( ( delta * delta / squaredDistance ) > marginSqrd ) )
|
|
{
|
|
// Convex shapes do not overlap
|
|
// Returning true means that Hybrid's result is ok and there's no need to run EPA
|
|
penDepth = 0;
|
|
return true;
|
|
}
|
|
|
|
//exit 0: the new point is already in the simplex, or we didn't come any closer
|
|
if ( ( squaredDistance - delta <= squaredDistance * g_GJKMaxRelErrorSqrd ) || simplexSolver.inSimplex( w ) )
|
|
{
|
|
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
|
|
|
|
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
|
|
btScalar vLength = sqrt( squaredDistance );
|
|
|
|
wWitnessOnA -= v * ( pConvexA->getMargin() / vLength );
|
|
wWitnessOnB += v * ( pConvexB->getMargin() / vLength );
|
|
|
|
penDepth = pConvexA->getMargin() + pConvexB->getMargin() - vLength;
|
|
|
|
// Returning true means that Hybrid's result is ok and there's no need to run EPA
|
|
return true;
|
|
}
|
|
|
|
//add current vertex to simplex
|
|
simplexSolver.addVertex( w, pWorld, qWorld );
|
|
|
|
//calculate the closest point to the origin (update vector v)
|
|
if ( !simplexSolver.closest( v ) )
|
|
{
|
|
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
|
|
|
|
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
|
|
btScalar vLength = sqrt( squaredDistance );
|
|
|
|
wWitnessOnA -= v * ( pConvexA->getMargin() / vLength );
|
|
wWitnessOnB += v * ( pConvexB->getMargin() / vLength );
|
|
|
|
penDepth = pConvexA->getMargin() + pConvexB->getMargin() - vLength;
|
|
|
|
// Returning true means that Hybrid's result is ok and there's no need to run EPA
|
|
return true;
|
|
}
|
|
|
|
btScalar previousSquaredDistance = squaredDistance;
|
|
squaredDistance = v.length2();
|
|
|
|
//are we getting any closer ?
|
|
if ( previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance )
|
|
{
|
|
simplexSolver.backup_closest( v );
|
|
squaredDistance = v.length2();
|
|
|
|
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
|
|
|
|
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
|
|
btScalar vLength = sqrt( squaredDistance );
|
|
|
|
wWitnessOnA -= v * ( pConvexA->getMargin() / vLength );
|
|
wWitnessOnB += v * ( pConvexB->getMargin() / vLength );
|
|
|
|
penDepth = pConvexA->getMargin() + pConvexB->getMargin() - vLength;
|
|
|
|
// Returning true means that Hybrid's result is ok and there's no need to run EPA
|
|
return true;
|
|
}
|
|
|
|
if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) )
|
|
{
|
|
// Convex Shapes intersect - we need to run EPA
|
|
// Returning false means that Hybrid couldn't do anything for us
|
|
// and that we need to run EPA to calculate the pen depth
|
|
return false;
|
|
}
|
|
|
|
++nbIterations;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
btScalar EpaPenetrationDepthSolver::EpaPenDepth( btSimplexSolverInterface& simplexSolver,
|
|
btConvexShape* pConvexA, btConvexShape* pConvexB,
|
|
const btTransform& transformA, const btTransform& transformB,
|
|
btPoint3& wWitnessOnA, btPoint3& wWitnessOnB )
|
|
{
|
|
Epa epa( pConvexA, pConvexB, transformA, transformB );
|
|
|
|
if ( !epa.Initialize( simplexSolver ) )
|
|
{
|
|
EPA_DEBUG_ASSERT( false ,"Epa failed to initialize!" );
|
|
return 0;
|
|
}
|
|
|
|
return epa.calcPenDepth( wWitnessOnA, wWitnessOnB );
|
|
}
|
|
|