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

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

@@ -32,24 +32,38 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim
(void)v; (void)v;
(void)simplexSolver; (void)simplexSolver;
// const btScalar radialmargin(btScalar(0.)); 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),
};
int numVectors = sizeof(guessVectors) / sizeof(btVector3);
for (int i = 0; i < numVectors; i++)
{
simplexSolver.reset();
btVector3 guessVector = guessVectors[i];
btVector3 guessVector(transformB.getOrigin()-transformA.getOrigin());
btGjkEpaSolver2::sResults results; btGjkEpaSolver2::sResults results;
if (btGjkEpaSolver2::Penetration(pConvexA, transformA, if (btGjkEpaSolver2::Penetration(pConvexA, transformA,
pConvexB, transformB, pConvexB, transformB,
guessVector, results)) 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]; wWitnessOnA = results.witnesses[0];
wWitnessOnB = results.witnesses[1]; wWitnessOnB = results.witnesses[1];
v = results.normal; v = results.normal;
return true; return true;
} else }
else
{ {
if (btGjkEpaSolver2::Distance(pConvexA, transformA, pConvexB, transformB, guessVector, results)) if (btGjkEpaSolver2::Distance(pConvexA, transformA, pConvexB, transformB, guessVector, results))
{ {
@@ -59,8 +73,12 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim
return false; 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; return false;
} }

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

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