Merge pull request #24740 from OBKF/update-bullet-physics

Update Bullet physics to commit 126b676

1 files changed, 233 insertions, 261 deletions

diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
index a0b825f0e8..803f6e0671 100644
--- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
@@ -18,75 +18,69 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
 
-
-
-#if defined(DEBUG) || defined (_DEBUG)
+#if defined(DEBUG) || defined(_DEBUG)
 //#define TEST_NON_VIRTUAL 1
-#include <stdio.h> //for debug printf
+#include <stdio.h>  //for debug printf
 #ifdef __SPU__
 #include <spu_printf.h>
 #define printf spu_printf
-#endif //__SPU__
+#endif  //__SPU__
 #endif
 
 //must be above the machine epsilon
-#ifdef  BT_USE_DOUBLE_PRECISION
-	#define REL_ERROR2 btScalar(1.0e-12)
-	btScalar gGjkEpaPenetrationTolerance = 1.0e-12;
+#ifdef BT_USE_DOUBLE_PRECISION
+#define REL_ERROR2 btScalar(1.0e-12)
+btScalar gGjkEpaPenetrationTolerance = 1.0e-12;
 #else
-	#define REL_ERROR2 btScalar(1.0e-6)
-	btScalar gGjkEpaPenetrationTolerance = 0.001;
+#define REL_ERROR2 btScalar(1.0e-6)
+btScalar gGjkEpaPenetrationTolerance = 0.001;
 #endif
 
-//temp globals, to improve GJK/EPA/penetration calculations
-int gNumDeepPenetrationChecks = 0;
-int gNumGjkChecks = 0;
-
-
-btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver)
-:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
-m_penetrationDepthSolver(penetrationDepthSolver),
-m_simplexSolver(simplexSolver),
-m_minkowskiA(objectA),
-m_minkowskiB(objectB),
-m_shapeTypeA(objectA->getShapeType()),
-m_shapeTypeB(objectB->getShapeType()),
-m_marginA(objectA->getMargin()),
-m_marginB(objectB->getMargin()),
-m_ignoreMargin(false),
-m_lastUsedMethod(-1),
-m_catchDegeneracies(1),
-m_fixContactNormalDirection(1)
+
+btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
+	: m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.)),
+	  m_penetrationDepthSolver(penetrationDepthSolver),
+	  m_simplexSolver(simplexSolver),
+	  m_minkowskiA(objectA),
+	  m_minkowskiB(objectB),
+	  m_shapeTypeA(objectA->getShapeType()),
+	  m_shapeTypeB(objectB->getShapeType()),
+	  m_marginA(objectA->getMargin()),
+	  m_marginB(objectB->getMargin()),
+	  m_ignoreMargin(false),
+	  m_lastUsedMethod(-1),
+	  m_catchDegeneracies(1),
+	  m_fixContactNormalDirection(1)
 {
 }
-btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver)
-:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
-m_penetrationDepthSolver(penetrationDepthSolver),
-m_simplexSolver(simplexSolver),
-m_minkowskiA(objectA),
-m_minkowskiB(objectB),
-m_shapeTypeA(shapeTypeA),
-m_shapeTypeB(shapeTypeB),
-m_marginA(marginA),
-m_marginB(marginB),
-m_ignoreMargin(false),
-m_lastUsedMethod(-1),
-m_catchDegeneracies(1),
-m_fixContactNormalDirection(1)
+btGjkPairDetector::btGjkPairDetector(const btConvexShape *objectA, const btConvexShape *objectB, int shapeTypeA, int shapeTypeB, btScalar marginA, btScalar marginB, btSimplexSolverInterface *simplexSolver, btConvexPenetrationDepthSolver *penetrationDepthSolver)
+	: m_cachedSeparatingAxis(btScalar(0.), btScalar(1.), btScalar(0.)),
+	  m_penetrationDepthSolver(penetrationDepthSolver),
+	  m_simplexSolver(simplexSolver),
+	  m_minkowskiA(objectA),
+	  m_minkowskiB(objectB),
+	  m_shapeTypeA(shapeTypeA),
+	  m_shapeTypeB(shapeTypeB),
+	  m_marginA(marginA),
+	  m_marginB(marginB),
+	  m_ignoreMargin(false),
+	  m_lastUsedMethod(-1),
+	  m_catchDegeneracies(1),
+	  m_fixContactNormalDirection(1)
 {
 }
 
-void	btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
+void btGjkPairDetector::getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults)
 {
 	(void)swapResults;
 
-	getClosestPointsNonVirtual(input,output,debugDraw);
+	getClosestPointsNonVirtual(input, output, debugDraw);
 }
 
-static void btComputeSupport(const btConvexShape* convexA, const btTransform& localTransA, const btConvexShape* convexB, const btTransform& localTransB, const btVector3& dir, bool check2d, btVector3& supAworld, btVector3& supBworld, btVector3& aMinb)
+static void btComputeSupport(const btConvexShape *convexA, const btTransform &localTransA, const btConvexShape *convexB, const btTransform &localTransB, const btVector3 &dir, bool check2d, btVector3 &supAworld, btVector3 &supBworld, btVector3 &aMinb)
 {
-	btVector3 seperatingAxisInA = (dir)* localTransA.getBasis();
-	btVector3 seperatingAxisInB = (-dir)* localTransB.getBasis();
+	btVector3 seperatingAxisInA = (dir)*localTransA.getBasis();
+	btVector3 seperatingAxisInB = (-dir) * localTransB.getBasis();
 
 	btVector3 pInANoMargin = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
 	btVector3 qInBNoMargin = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
@@ -106,22 +100,21 @@ static void btComputeSupport(const btConvexShape* convexA, const btTransform& lo
 	aMinb = supAworld - supBworld;
 }
 
-struct btSupportVector 
+struct btSupportVector
 {
-	btVector3 v;  //!< Support point in minkowski sum
-	btVector3 v1; //!< Support point in obj1
-	btVector3 v2; //!< Support point in obj2
+	btVector3 v;   //!< Support point in minkowski sum
+	btVector3 v1;  //!< Support point in obj1
+	btVector3 v2;  //!< Support point in obj2
 };
 
-struct btSimplex 
+struct btSimplex
 {
 	btSupportVector ps[4];
-	int last; //!< index of last added point
+	int last;  //!< index of last added point
 };
 
 static btVector3 ccd_vec3_origin(0, 0, 0);
 
-
 inline void btSimplexInit(btSimplex *s)
 {
 	s->last = -1;
@@ -142,19 +135,18 @@ inline void btSupportCopy(btSupportVector *d, const btSupportVector *s)
 	*d = *s;
 }
 
-inline  void btVec3Copy(btVector3 *v, const btVector3* w)
+inline void btVec3Copy(btVector3 *v, const btVector3 *w)
 {
 	*v = *w;
 }
 
-inline void ccdVec3Add(btVector3*v, const btVector3*w)
+inline void ccdVec3Add(btVector3 *v, const btVector3 *w)
 {
 	v->m_floats[0] += w->m_floats[0];
 	v->m_floats[1] += w->m_floats[1];
 	v->m_floats[2] += w->m_floats[2];
 }
 
-
 inline void ccdVec3Sub(btVector3 *v, const btVector3 *w)
 {
 	*v -= *w;
@@ -162,24 +154,22 @@ inline void ccdVec3Sub(btVector3 *v, const btVector3 *w)
 inline void btVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
 {
 	*d = (*v) - (*w);
-	
 }
-inline  btScalar btVec3Dot(const btVector3 *a, const btVector3 *b)
+inline btScalar btVec3Dot(const btVector3 *a, const btVector3 *b)
 {
 	btScalar dot;
 	dot = a->dot(*b);
-	
+
 	return dot;
 }
 
-inline btScalar ccdVec3Dist2(const btVector3 *a, const btVector3*b)
+inline btScalar ccdVec3Dist2(const btVector3 *a, const btVector3 *b)
 {
 	btVector3 ab;
 	btVec3Sub2(&ab, a, b);
 	return btVec3Dot(&ab, &ab);
 }
 
-
 inline void btVec3Scale(btVector3 *d, btScalar k)
 {
 	d->m_floats[0] *= k;
@@ -195,7 +185,7 @@ inline void btVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
 }
 
 inline void btTripleCross(const btVector3 *a, const btVector3 *b,
-	const btVector3 *c, btVector3 *d)
+						  const btVector3 *c, btVector3 *d)
 {
 	btVector3 e;
 	btVec3Cross(&e, a, b);
@@ -213,36 +203,35 @@ inline int ccdEq(btScalar _a, btScalar _b)
 
 	a = btFabs(_a);
 	b = btFabs(_b);
-	if (b > a) {
+	if (b > a)
+	{
 		return ab < SIMD_EPSILON * b;
 	}
-	else {
+	else
+	{
 		return ab < SIMD_EPSILON * a;
 	}
 }
 
-btScalar ccdVec3X(const btVector3* v)
+btScalar ccdVec3X(const btVector3 *v)
 {
 	return v->x();
 }
 
-btScalar ccdVec3Y(const btVector3* v)
+btScalar ccdVec3Y(const btVector3 *v)
 {
 	return v->y();
 }
 
-btScalar ccdVec3Z(const btVector3* v)
+btScalar ccdVec3Z(const btVector3 *v)
 {
 	return v->z();
 }
 inline int btVec3Eq(const btVector3 *a, const btVector3 *b)
 {
-	return ccdEq(ccdVec3X(a), ccdVec3X(b))
-		&& ccdEq(ccdVec3Y(a), ccdVec3Y(b))
-		&& ccdEq(ccdVec3Z(a), ccdVec3Z(b));
+	return ccdEq(ccdVec3X(a), ccdVec3X(b)) && ccdEq(ccdVec3Y(a), ccdVec3Y(b)) && ccdEq(ccdVec3Z(a), ccdVec3Z(b));
 }
 
-
 inline void btSimplexAdd(btSimplex *s, const btSupportVector *v)
 {
 	// here is no check on boundaries in sake of speed
@@ -250,7 +239,6 @@ inline void btSimplexAdd(btSimplex *s, const btSupportVector *v)
 	btSupportCopy(s->ps + s->last, v);
 }
 
-
 inline void btSimplexSet(btSimplex *s, size_t pos, const btSupportVector *a)
 {
 	btSupportCopy(s->ps + pos, a);
@@ -268,27 +256,28 @@ inline const btSupportVector *ccdSimplexLast(const btSimplex *s)
 
 inline int ccdSign(btScalar val)
 {
-	if (btFuzzyZero(val)) {
+	if (btFuzzyZero(val))
+	{
 		return 0;
 	}
-	else if (val < btScalar(0)) {
+	else if (val < btScalar(0))
+	{
 		return -1;
 	}
 	return 1;
 }
 
-
 inline btScalar btVec3PointSegmentDist2(const btVector3 *P,
-	const btVector3 *x0,
-	const btVector3 *b,
-	btVector3 *witness)
+										const btVector3 *x0,
+										const btVector3 *b,
+										btVector3 *witness)
 {
 	// The computation comes from solving equation of segment:
 	//      S(t) = x0 + t.d
 	//          where - x0 is initial point of segment
 	//                - d is direction of segment from x0 (|d| > 0)
 	//                - t belongs to <0, 1> interval
-	// 
+	//
 	// Than, distance from a segment to some point P can be expressed:
 	//      D(t) = |x0 + t.d - P|^2
 	//          which is distance from any point on segment. Minimization
@@ -310,24 +299,29 @@ inline btScalar btVec3PointSegmentDist2(const btVector3 *P,
 	t = -btScalar(1.) * btVec3Dot(&a, &d);
 	t /= btVec3Dot(&d, &d);
 
-	if (t < btScalar(0) || btFuzzyZero(t)) {
+	if (t < btScalar(0) || btFuzzyZero(t))
+	{
 		dist = ccdVec3Dist2(x0, P);
 		if (witness)
 			btVec3Copy(witness, x0);
 	}
-	else if (t > btScalar(1) || ccdEq(t, btScalar(1))) {
+	else if (t > btScalar(1) || ccdEq(t, btScalar(1)))
+	{
 		dist = ccdVec3Dist2(b, P);
 		if (witness)
 			btVec3Copy(witness, b);
 	}
-	else {
-		if (witness) {
+	else
+	{
+		if (witness)
+		{
 			btVec3Copy(witness, &d);
 			btVec3Scale(witness, t);
 			ccdVec3Add(witness, x0);
 			dist = ccdVec3Dist2(witness, P);
 		}
-		else {
+		else
+		{
 			// recycling variables
 			btVec3Scale(&d, t);
 			ccdVec3Add(&d, &a);
@@ -338,11 +332,10 @@ inline btScalar btVec3PointSegmentDist2(const btVector3 *P,
 	return dist;
 }
 
-
 btScalar btVec3PointTriDist2(const btVector3 *P,
-	const btVector3 *x0, const btVector3 *B,
-	const btVector3 *C,
-	btVector3 *witness)
+							 const btVector3 *x0, const btVector3 *B,
+							 const btVector3 *C,
+							 btVector3 *witness)
 {
 	// Computation comes from analytic expression for triangle (x0, B, C)
 	//      T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
@@ -372,13 +365,9 @@ btScalar btVec3PointTriDist2(const btVector3 *P,
 	s = (q * r - w * p) / (w * v - r * r);
 	t = (-s * r - q) / w;
 
-	if ((btFuzzyZero(s) || s > btScalar(0))
-		&& (ccdEq(s, btScalar(1)) || s < btScalar(1))
-		&& (btFuzzyZero(t) || t > btScalar(0))
-		&& (ccdEq(t, btScalar(1)) || t < btScalar(1))
-		&& (ccdEq(t + s, btScalar(1)) || t + s < btScalar(1))) {
-
-		if (witness) 
+	if ((btFuzzyZero(s) || s > btScalar(0)) && (ccdEq(s, btScalar(1)) || s < btScalar(1)) && (btFuzzyZero(t) || t > btScalar(0)) && (ccdEq(t, btScalar(1)) || t < btScalar(1)) && (ccdEq(t + s, btScalar(1)) || t + s < btScalar(1)))
+	{
+		if (witness)
 		{
 			btVec3Scale(&d1, s);
 			btVec3Scale(&d2, t);
@@ -388,7 +377,7 @@ btScalar btVec3PointTriDist2(const btVector3 *P,
 
 			dist = ccdVec3Dist2(witness, P);
 		}
-		else 
+		else
 		{
 			dist = s * s * v;
 			dist += t * t * w;
@@ -398,18 +387,21 @@ btScalar btVec3PointTriDist2(const btVector3 *P,
 			dist += u;
 		}
 	}
-	else {
+	else
+	{
 		dist = btVec3PointSegmentDist2(P, x0, B, witness);
 
 		dist2 = btVec3PointSegmentDist2(P, x0, C, &witness2);
-		if (dist2 < dist) {
+		if (dist2 < dist)
+		{
 			dist = dist2;
 			if (witness)
 				btVec3Copy(witness, &witness2);
 		}
 
 		dist2 = btVec3PointSegmentDist2(P, B, C, &witness2);
-		if (dist2 < dist) {
+		if (dist2 < dist)
+		{
 			dist = dist2;
 			if (witness)
 				btVec3Copy(witness, &witness2);
@@ -419,7 +411,6 @@ btScalar btVec3PointTriDist2(const btVector3 *P,
 	return dist;
 }
 
-
 static int btDoSimplex2(btSimplex *simplex, btVector3 *dir)
 {
 	const btSupportVector *A, *B;
@@ -441,18 +432,21 @@ static int btDoSimplex2(btSimplex *simplex, btVector3 *dir)
 
 	// check if origin doesn't lie on AB segment
 	btVec3Cross(&tmp, &AB, &AO);
-	if (btFuzzyZero(btVec3Dot(&tmp, &tmp)) && dot > btScalar(0)) {
+	if (btFuzzyZero(btVec3Dot(&tmp, &tmp)) && dot > btScalar(0))
+	{
 		return 1;
 	}
 
 	// check if origin is in area where AB segment is
-	if (btFuzzyZero(dot) || dot < btScalar(0)) {
+	if (btFuzzyZero(dot) || dot < btScalar(0))
+	{
 		// origin is in outside are of A
 		btSimplexSet(simplex, 0, A);
 		btSimplexSetSize(simplex, 1);
 		btVec3Copy(dir, &AO);
 	}
-	else {
+	else
+	{
 		// origin is in area where AB segment is
 
 		// keep simplex untouched and set direction to
@@ -463,8 +457,6 @@ static int btDoSimplex2(btSimplex *simplex, btVector3 *dir)
 	return 0;
 }
 
-
-
 static int btDoSimplex3(btSimplex *simplex, btVector3 *dir)
 {
 	const btSupportVector *A, *B, *C;
@@ -479,13 +471,15 @@ static int btDoSimplex3(btSimplex *simplex, btVector3 *dir)
 
 	// check touching contact
 	dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0);
-	if (btFuzzyZero(dist)) {
+	if (btFuzzyZero(dist))
+	{
 		return 1;
 	}
 
 	// check if triangle is really triangle (has area > 0)
 	// if not simplex can't be expanded and thus no itersection is found
-	if (btVec3Eq(&A->v, &B->v) || btVec3Eq(&A->v, &C->v)) {
+	if (btVec3Eq(&A->v, &B->v) || btVec3Eq(&A->v, &C->v))
+	{
 		return -1;
 	}
 
@@ -500,54 +494,64 @@ static int btDoSimplex3(btSimplex *simplex, btVector3 *dir)
 
 	btVec3Cross(&tmp, &ABC, &AC);
 	dot = btVec3Dot(&tmp, &AO);
-	if (btFuzzyZero(dot) || dot > btScalar(0)) {
+	if (btFuzzyZero(dot) || dot > btScalar(0))
+	{
 		dot = btVec3Dot(&AC, &AO);
-		if (btFuzzyZero(dot) || dot > btScalar(0)) {
+		if (btFuzzyZero(dot) || dot > btScalar(0))
+		{
 			// C is already in place
 			btSimplexSet(simplex, 1, A);
 			btSimplexSetSize(simplex, 2);
 			btTripleCross(&AC, &AO, &AC, dir);
 		}
-		else {
-		
+		else
+		{
 			dot = btVec3Dot(&AB, &AO);
-			if (btFuzzyZero(dot) || dot > btScalar(0)) {
+			if (btFuzzyZero(dot) || dot > btScalar(0))
+			{
 				btSimplexSet(simplex, 0, B);
 				btSimplexSet(simplex, 1, A);
 				btSimplexSetSize(simplex, 2);
 				btTripleCross(&AB, &AO, &AB, dir);
 			}
-			else {
+			else
+			{
 				btSimplexSet(simplex, 0, A);
 				btSimplexSetSize(simplex, 1);
 				btVec3Copy(dir, &AO);
 			}
 		}
 	}
-	else {
+	else
+	{
 		btVec3Cross(&tmp, &AB, &ABC);
 		dot = btVec3Dot(&tmp, &AO);
-		if (btFuzzyZero(dot) || dot > btScalar(0)) 
+		if (btFuzzyZero(dot) || dot > btScalar(0))
 		{
 			dot = btVec3Dot(&AB, &AO);
-			if (btFuzzyZero(dot) || dot > btScalar(0)) {
+			if (btFuzzyZero(dot) || dot > btScalar(0))
+			{
 				btSimplexSet(simplex, 0, B);
 				btSimplexSet(simplex, 1, A);
 				btSimplexSetSize(simplex, 2);
 				btTripleCross(&AB, &AO, &AB, dir);
 			}
-			else {
+			else
+			{
 				btSimplexSet(simplex, 0, A);
 				btSimplexSetSize(simplex, 1);
 				btVec3Copy(dir, &AO);
 			}
 		}
-		else {
+		else
+		{
 			dot = btVec3Dot(&ABC, &AO);
-			if (btFuzzyZero(dot) || dot > btScalar(0)) {
+			if (btFuzzyZero(dot) || dot > btScalar(0))
+			{
 				btVec3Copy(dir, &ABC);
 			}
-			else {
+			else
+			{
 				btSupportVector tmp;
 				btSupportCopy(&tmp, C);
 				btSimplexSet(simplex, 0, B);
@@ -581,7 +585,8 @@ static int btDoSimplex4(btSimplex *simplex, btVector3 *dir)
 	// if it is not simplex can't be expanded and thus no intersection is
 	// found
 	dist = btVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, 0);
-	if (btFuzzyZero(dist)) {
+	if (btFuzzyZero(dist))
+	{
 		return -1;
 	}
 
@@ -622,12 +627,14 @@ static int btDoSimplex4(btSimplex *simplex, btVector3 *dir)
 	AC_O = ccdSign(btVec3Dot(&ADB, &AO)) == C_on_ADB;
 	AD_O = ccdSign(btVec3Dot(&ABC, &AO)) == D_on_ABC;
 
-	if (AB_O && AC_O && AD_O) {
+	if (AB_O && AC_O && AD_O)
+	{
 		// origin is in tetrahedron
 		return 1;
 		// rearrange simplex to triangle and call btDoSimplex3()
 	}
-	else if (!AB_O) {
+	else if (!AB_O)
+	{
 		// B is farthest from the origin among all of the tetrahedron's
 		// points, so remove it from the list and go on with the triangle
 		// case
@@ -636,14 +643,16 @@ static int btDoSimplex4(btSimplex *simplex, btVector3 *dir)
 		btSimplexSet(simplex, 2, A);
 		btSimplexSetSize(simplex, 3);
 	}
-	else if (!AC_O) {
+	else if (!AC_O)
+	{
 		// C is farthest
 		btSimplexSet(simplex, 1, D);
 		btSimplexSet(simplex, 0, B);
 		btSimplexSet(simplex, 2, A);
 		btSimplexSetSize(simplex, 3);
 	}
-	else { // (!AD_O)
+	else
+	{  // (!AD_O)
 		btSimplexSet(simplex, 0, C);
 		btSimplexSet(simplex, 1, B);
 		btSimplexSet(simplex, 2, A);
@@ -655,36 +664,39 @@ static int btDoSimplex4(btSimplex *simplex, btVector3 *dir)
 
 static int btDoSimplex(btSimplex *simplex, btVector3 *dir)
 {
-	if (btSimplexSize(simplex) == 2) {
+	if (btSimplexSize(simplex) == 2)
+	{
 		// simplex contains segment only one segment
 		return btDoSimplex2(simplex, dir);
 	}
-	else if (btSimplexSize(simplex) == 3) {
+	else if (btSimplexSize(simplex) == 3)
+	{
 		// simplex contains triangle
 		return btDoSimplex3(simplex, dir);
 	}
-	else { // btSimplexSize(simplex) == 4
-			// tetrahedron - this is the only shape which can encapsule origin
-			// so btDoSimplex4() also contains test on it
+	else
+	{  // btSimplexSize(simplex) == 4
+		// tetrahedron - this is the only shape which can encapsule origin
+		// so btDoSimplex4() also contains test on it
 		return btDoSimplex4(simplex, dir);
 	}
 }
 
 #ifdef __SPU__
-void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
+void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw)
 #else
-void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw)
+void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw)
 #endif
 {
 	m_cachedSeparatingDistance = 0.f;
 
-	btScalar distance=btScalar(0.);
-	btVector3	normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
+	btScalar distance = btScalar(0.);
+	btVector3 normalInB(btScalar(0.), btScalar(0.), btScalar(0.));
 
-	btVector3 pointOnA,pointOnB;
-	btTransform	localTransA = input.m_transformA;
+	btVector3 pointOnA, pointOnB;
+	btTransform localTransA = input.m_transformA;
 	btTransform localTransB = input.m_transformB;
-	btVector3 positionOffset=(localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
+	btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
 	localTransA.getOrigin() -= positionOffset;
 	localTransB.getOrigin() -= positionOffset;
 
@@ -693,7 +705,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 	btScalar marginA = m_marginA;
 	btScalar marginB = m_marginB;
 
-	gNumGjkChecks++;
 
 	//for CCD we don't use margins
 	if (m_ignoreMargin)
@@ -703,19 +714,19 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 	}
 
 	m_curIter = 0;
-	int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
-	m_cachedSeparatingAxis.setValue(0,1,0);
+	int gGjkMaxIter = 1000;  //this is to catch invalid input, perhaps check for #NaN?
+	m_cachedSeparatingAxis.setValue(0, 1, 0);
 
 	bool isValid = false;
 	bool checkSimplex = false;
 	bool checkPenetration = true;
 	m_degenerateSimplex = 0;
-	
+
 	m_lastUsedMethod = -1;
 	int status = -2;
 	btVector3 orgNormalInB(0, 0, 0);
 	btScalar margin = marginA + marginB;
-	
+
 	//we add a separate implementation to check if the convex shapes intersect
 	//See also "Real-time Collision Detection with Implicit Objects" by Leif Olvang
 	//Todo: integrate the simplex penetration check directly inside the Bullet btVoronoiSimplexSolver
@@ -726,22 +737,18 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 		btScalar squaredDistance = BT_LARGE_FLOAT;
 		btScalar delta = btScalar(0.);
 
-
-
-		
 		btSimplex simplex1;
-		btSimplex* simplex = &simplex1;
+		btSimplex *simplex = &simplex1;
 		btSimplexInit(simplex);
 
 		btVector3 dir(1, 0, 0);
 
 		{
-
 			btVector3 lastSupV;
 			btVector3 supAworld;
 			btVector3 supBworld;
 			btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
-			
+
 			btSupportVector last;
 			last.v = lastSupV;
 			last.v1 = supAworld;
@@ -751,10 +758,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 
 			dir = -lastSupV;
 
-			
-			
 			// start iterations
-			for (int iterations = 0; iterations <gGjkMaxIter; iterations++)
+			for (int iterations = 0; iterations < gGjkMaxIter; iterations++)
 			{
 				// obtain support point
 				btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
@@ -769,7 +774,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 					status = -1;
 					break;
 				}
-				
+
 				// add last support vector to simplex
 				last.v = lastSupV;
 				last.v1 = supAworld;
@@ -781,21 +786,21 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 				// intersect and 0 if algorithm should continue
 
 				btVector3 newDir;
-				int do_simplex_res = btDoSimplex(simplex, &dir); 
+				int do_simplex_res = btDoSimplex(simplex, &dir);
 
-				if (do_simplex_res == 1) 
+				if (do_simplex_res == 1)
 				{
-					status = 0; // intersection found
+					status = 0;  // intersection found
 					break;
 				}
-				else if (do_simplex_res == -1) 
+				else if (do_simplex_res == -1)
 				{
 					// intersection not found
 					status = -1;
 					break;
 				}
-				
-				if (btFuzzyZero(btVec3Dot(&dir, &dir))) 
+
+				if (btFuzzyZero(btVec3Dot(&dir, &dir)))
 				{
 					// intersection not found
 					status = -1;
@@ -815,7 +820,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 					break;
 				}
 			}
-
 		}
 
 		m_simplexSolver->reset();
@@ -825,27 +829,24 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 			//printf("Intersect!\n");
 		}
 
-		if (status==-1)
+		if (status == -1)
 		{
 			//printf("not intersect\n");
 		}
 		//printf("dir=%f,%f,%f\n",dir[0],dir[1],dir[2]);
 		if (1)
 		{
-			for (; ; )
-				//while (true)
+			for (;;)
+			//while (true)
 			{
-
-				btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* localTransA.getBasis();
-				btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis();
-
+				btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis();
+				btVector3 seperatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis();
 
 				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
 				btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
 
-				btVector3  pWorld = localTransA(pInA);
-				btVector3  qWorld = localTransB(qInB);
-
+				btVector3 pWorld = localTransA(pInA);
+				btVector3 qWorld = localTransB(qInB);
 
 				if (check2d)
 				{
@@ -921,8 +922,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 					checkSimplex = false;
 					break;
 				}
-#endif //
-
+#endif  //
 
 				//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
 
@@ -938,33 +938,31 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 
 				m_cachedSeparatingAxis = newCachedSeparatingAxis;
 
-				//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject   
+				//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
 				if (m_curIter++ > gGjkMaxIter)
 				{
-#if defined(DEBUG) || defined (_DEBUG)
+#if defined(DEBUG) || defined(_DEBUG)
 
 					printf("btGjkPairDetector maxIter exceeded:%i\n", m_curIter);
 					printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
-						m_cachedSeparatingAxis.getX(),
-						m_cachedSeparatingAxis.getY(),
-						m_cachedSeparatingAxis.getZ(),
-						squaredDistance,
-						m_minkowskiA->getShapeType(),
-						m_minkowskiB->getShapeType());
-
-#endif   
-					break;
+						   m_cachedSeparatingAxis.getX(),
+						   m_cachedSeparatingAxis.getY(),
+						   m_cachedSeparatingAxis.getZ(),
+						   squaredDistance,
+						   m_minkowskiA->getShapeType(),
+						   m_minkowskiB->getShapeType());
 
+#endif
+					break;
 				}
 
-
 				bool check = (!m_simplexSolver->fullSimplex());
 				//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
 
 				if (!check)
 				{
 					//do we need this backup_closest here ?
-	//				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+					//				m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
 					m_degenerateSimplex = 13;
 					break;
 				}
@@ -972,20 +970,20 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 
 			if (checkSimplex)
 			{
-			m_simplexSolver->compute_points(pointOnA, pointOnB);
-			normalInB = m_cachedSeparatingAxis;
+				m_simplexSolver->compute_points(pointOnA, pointOnB);
+				normalInB = m_cachedSeparatingAxis;
+
+				btScalar lenSqr = m_cachedSeparatingAxis.length2();
 
-			btScalar lenSqr =m_cachedSeparatingAxis.length2();
-			
 				//valid normal
 				if (lenSqr < REL_ERROR2)
 				{
 					m_degenerateSimplex = 5;
 				}
-				if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
+				if (lenSqr > SIMD_EPSILON * SIMD_EPSILON)
 				{
 					btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
-					normalInB *= rlen; //normalize
+					normalInB *= rlen;  //normalize
 
 					btScalar s = btSqrt(squaredDistance);
 
@@ -1005,13 +1003,11 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 			}
 		}
 
-
-		
-		bool catchDegeneratePenetrationCase = 
-			(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance));
+		bool catchDegeneratePenetrationCase =
+			(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance + margin) < gGjkEpaPenetrationTolerance));
 
 		//if (checkPenetration && !isValid)
-		if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase )) || (status == 0))
+		if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase)) || (status == 0))
 		{
 			//penetration case
 
@@ -1019,19 +1015,16 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 			if (m_penetrationDepthSolver)
 			{
 				// Penetration depth case.
-				btVector3 tmpPointOnA,tmpPointOnB;
-				
-				gNumDeepPenetrationChecks++;
+				btVector3 tmpPointOnA, tmpPointOnB;
+
 				m_cachedSeparatingAxis.setZero();
 
-				bool isValid2 = m_penetrationDepthSolver->calcPenDepth( 
-					*m_simplexSolver, 
-					m_minkowskiA,m_minkowskiB,
-					localTransA,localTransB,
+				bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
+					*m_simplexSolver,
+					m_minkowskiA, m_minkowskiB,
+					localTransA, localTransB,
 					m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
-					debugDraw
-					);
-
+					debugDraw);
 
 				if (m_cachedSeparatingAxis.length2())
 				{
@@ -1039,13 +1032,13 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 					{
 						btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
 						btScalar lenSqr = tmpNormalInB.length2();
-						if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
+						if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON))
 						{
 							tmpNormalInB = m_cachedSeparatingAxis;
 							lenSqr = m_cachedSeparatingAxis.length2();
 						}
 
-						if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+						if (lenSqr > (SIMD_EPSILON * SIMD_EPSILON))
 						{
 							tmpNormalInB /= btSqrt(lenSqr);
 							btScalar distance2 = -(tmpPointOnA - tmpPointOnB).length();
@@ -1058,7 +1051,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 								pointOnB = tmpPointOnB;
 								normalInB = tmpNormalInB;
 								isValid = true;
-
 							}
 							else
 							{
@@ -1079,7 +1071,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 						///thanks to Jacob.Langford for the reproduction case
 						///http://code.google.com/p/bullet/issues/detail?id=250
 
-
 						if (m_cachedSeparatingAxis.length2() > btScalar(0.))
 						{
 							btScalar distance2 = (tmpPointOnA - tmpPointOnB).length() - margin;
@@ -1103,109 +1094,90 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 							}
 						}
 					}
-				} else
+				}
+				else
 				{
 					//printf("EPA didn't return a valid value\n");
 				}
-				
 			}
-
 		}
 	}
 
-	
-
-	if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
+	if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared)))
 	{
-
 		m_cachedSeparatingAxis = normalInB;
 		m_cachedSeparatingDistance = distance;
 		if (1)
 		{
-		///todo: need to track down this EPA penetration solver degeneracy
-		///the penetration solver reports penetration but the contact normal
-		///connecting the contact points is pointing in the opposite direction
-		///until then, detect the issue and revert the normal
+			///todo: need to track down this EPA penetration solver degeneracy
+			///the penetration solver reports penetration but the contact normal
+			///connecting the contact points is pointing in the opposite direction
+			///until then, detect the issue and revert the normal
 
 			btScalar d2 = 0.f;
 			{
-				btVector3 seperatingAxisInA = (-orgNormalInB)* localTransA.getBasis();
-				btVector3 seperatingAxisInB = orgNormalInB* localTransB.getBasis();
-
+				btVector3 seperatingAxisInA = (-orgNormalInB) * localTransA.getBasis();
+				btVector3 seperatingAxisInB = orgNormalInB * localTransB.getBasis();
 
 				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
 				btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
 
-				btVector3  pWorld = localTransA(pInA);
-				btVector3  qWorld = localTransB(qInB);
+				btVector3 pWorld = localTransA(pInA);
+				btVector3 qWorld = localTransB(qInB);
 				btVector3 w = pWorld - qWorld;
-				d2 = orgNormalInB.dot(w)- margin;
+				d2 = orgNormalInB.dot(w) - margin;
 			}
-			
-			btScalar d1=0;
+
+			btScalar d1 = 0;
 			{
-				
-				btVector3 seperatingAxisInA = (normalInB)* localTransA.getBasis();
-				btVector3 seperatingAxisInB = -normalInB* localTransB.getBasis();
-			
+				btVector3 seperatingAxisInA = (normalInB)*localTransA.getBasis();
+				btVector3 seperatingAxisInB = -normalInB * localTransB.getBasis();
 
 				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
 				btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
 
-				btVector3  pWorld = localTransA(pInA);	
-				btVector3  qWorld = localTransB(qInB);
-				btVector3 w	= pWorld - qWorld;
-				d1 = (-normalInB).dot(w)- margin;
-
+				btVector3 pWorld = localTransA(pInA);
+				btVector3 qWorld = localTransB(qInB);
+				btVector3 w = pWorld - qWorld;
+				d1 = (-normalInB).dot(w) - margin;
 			}
 			btScalar d0 = 0.f;
 			{
-				btVector3 seperatingAxisInA = (-normalInB)* input.m_transformA.getBasis();
-				btVector3 seperatingAxisInB = normalInB* input.m_transformB.getBasis();
-			
+				btVector3 seperatingAxisInA = (-normalInB) * input.m_transformA.getBasis();
+				btVector3 seperatingAxisInB = normalInB * input.m_transformB.getBasis();
 
 				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
 				btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
 
-				btVector3  pWorld = localTransA(pInA);	
-				btVector3  qWorld = localTransB(qInB);
-				btVector3 w	= pWorld - qWorld;
-				d0 = normalInB.dot(w)-margin;
+				btVector3 pWorld = localTransA(pInA);
+				btVector3 qWorld = localTransB(qInB);
+				btVector3 w = pWorld - qWorld;
+				d0 = normalInB.dot(w) - margin;
 			}
-			
-			if (d1>d0)
+
+			if (d1 > d0)
 			{
 				m_lastUsedMethod = 10;
-				normalInB*=-1;
-			} 
+				normalInB *= -1;
+			}
 
 			if (orgNormalInB.length2())
 			{
 				if (d2 > d0 && d2 > d1 && d2 > distance)
 				{
-
 					normalInB = orgNormalInB;
 					distance = d2;
 				}
 			}
 		}
 
-
 		output.addContactPoint(
 			normalInB,
-			pointOnB+positionOffset,
+			pointOnB + positionOffset,
 			distance);
-
 	}
 	else
 	{
 		//printf("invalid gjk query\n");
 	}
-
-
 }
-
-
-
-
-