1 files changed, 43 insertions, 25 deletions

diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
index 572ec36f56..b79f49d611 100644
--- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
@@ -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()).normalized(),
+		btVector3(transformA.getOrigin() - transformB.getOrigin()).normalized(),
+		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;
 }
 
-