Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

make gjk a bit more robust, try different initial guess vector if it fails to find a solution (happens for queries with large differences in shape size)

Browse Source
This commit is contained in:
Erwin Coumans
2018-05-25 08:18:12 +10:00
parent abeae7e1e7
commit a9ff5246c9
2 changed files with 44 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
View File

@@ -21,46 +21,64 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver,
const btConvexShape* pConvexA, const btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
class btIDebugDraw* debugDraw)
bool btGjkEpaPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
const btConvexShape* pConvexA, const btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
class btIDebugDraw* debugDraw)
{
(void)debugDraw;
(void)v;
(void)simplexSolver;
// const btScalar radialmargin(btScalar(0.));
btVector3 guessVector(transformB.getOrigin()-transformA.getOrigin());
btGjkEpaSolver2::sResults results;
btVector3 guessVectors[] = {
btVector3(transformB.getOrigin() - transformA.getOrigin()),
btVector3(transformA.getOrigin() - transformB.getOrigin()),
btVector3(0, 0, 1),
btVector3(0, 1, 0),
btVector3(1, 0, 0),
btVector3(1, 1, 0),
btVector3(1, 1, 1),
btVector3(0, 1, 1),
btVector3(1, 0, 1),
};
if(btGjkEpaSolver2::Penetration(pConvexA,transformA,
pConvexB,transformB,
guessVector,results))
{
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
wWitnessOnA = results.witnesses[0];
wWitnessOnB = results.witnesses[1];
v = results.normal;
return true;
} else
int numVectors = sizeof(guessVectors) / sizeof(btVector3);
for (int i = 0; i < numVectors; i++)
{
if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results))
simplexSolver.reset();
btVector3 guessVector = guessVectors[i];
btGjkEpaSolver2::sResults results;
if (btGjkEpaSolver2::Penetration(pConvexA, transformA,
pConvexB, transformB,
guessVector, results))
{
wWitnessOnA = results.witnesses[0];
wWitnessOnB = results.witnesses[1];
v = results.normal;
return false;
return true;
}
else
{
if (btGjkEpaSolver2::Distance(pConvexA, transformA, pConvexB, transformB, guessVector, results))
{
wWitnessOnA = results.witnesses[0];
wWitnessOnB = results.witnesses[1];
v = results.normal;
return false;
}
}
}
//failed to find a distance/penetration
wWitnessOnA.setValue(0, 0, 0);
wWitnessOnB.setValue(0, 0, 0);
v.setValue(0, 0, 0);
return false;
}

2
src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
View File

@@ -35,7 +35,7 @@ subject to the following restrictions:
btScalar gGjkEpaPenetrationTolerance = 1e-7;
#else
#define REL_ERROR2 btScalar(1.0e-6)
btScalar gGjkEpaPenetrationTolerance = 0.001;
btScalar gGjkEpaPenetrationTolerance = BT_LARGE_FLOAT;
#endif
//temp globals, to improve GJK/EPA/penetration calculations