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

Update Bullet physics to commit 126b676

1 files changed, 637 insertions, 588 deletions

diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp
index 7043bde34f..202039956e 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp
@@ -24,14 +24,12 @@ subject to the following restrictions:
 #include <float.h>
 #include <string.h>
 
-btBoxBoxDetector::btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2)
-: m_box1(box1),
-m_box2(box2)
+btBoxBoxDetector::btBoxBoxDetector(const btBoxShape* box1, const btBoxShape* box2)
+	: m_box1(box1),
+	  m_box2(box2)
 {
-
 }
 
-
 // given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
 // generate contact points. this returns 0 if there is no contact otherwise
 // it returns the number of contacts generated.
@@ -48,67 +46,66 @@ m_box2(box2)
 // collision functions. this function only fills in the position and depth
 // fields.
 struct dContactGeom;
-#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
+#define dDOTpq(a, b, p, q) ((a)[0] * (b)[0] + (a)[p] * (b)[q] + (a)[2 * (p)] * (b)[2 * (q)])
 #define dInfinity FLT_MAX
 
-
 /*PURE_INLINE btScalar dDOT   (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
 PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); }
 PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); }
 PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); }
 */
-static btScalar dDOT   (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
-static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); }
-static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); }
-static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); }
-#define dMULTIPLYOP1_331(A,op,B,C) \
-{\
-  (A)[0] op dDOT41((B),(C)); \
-  (A)[1] op dDOT41((B+1),(C)); \
-  (A)[2] op dDOT41((B+2),(C)); \
-}
+static btScalar dDOT(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 1, 1); }
+static btScalar dDOT44(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 4, 4); }
+static btScalar dDOT41(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 4, 1); }
+static btScalar dDOT14(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 1, 4); }
+#define dMULTIPLYOP1_331(A, op, B, C)   \
+	{                                   \
+		(A)[0] op dDOT41((B), (C));     \
+		(A)[1] op dDOT41((B + 1), (C)); \
+		(A)[2] op dDOT41((B + 2), (C)); \
+	}
 
-#define dMULTIPLYOP0_331(A,op,B,C) \
-{ \
-  (A)[0] op dDOT((B),(C)); \
-  (A)[1] op dDOT((B+4),(C)); \
-  (A)[2] op dDOT((B+8),(C)); \
-} 
+#define dMULTIPLYOP0_331(A, op, B, C) \
+	{                                 \
+		(A)[0] op dDOT((B), (C));     \
+		(A)[1] op dDOT((B + 4), (C)); \
+		(A)[2] op dDOT((B + 8), (C)); \
+	}
 
-#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C)
-#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C)
+#define dMULTIPLY1_331(A, B, C) dMULTIPLYOP1_331(A, =, B, C)
+#define dMULTIPLY0_331(A, B, C) dMULTIPLYOP0_331(A, =, B, C)
 
-typedef btScalar dMatrix3[4*3];
+typedef btScalar dMatrix3[4 * 3];
 
-void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
-			   const btVector3& pb, const btVector3& ub,
-			   btScalar *alpha, btScalar *beta);
-void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
-			   const btVector3& pb, const btVector3& ub,
-			   btScalar *alpha, btScalar *beta)
+void dLineClosestApproach(const btVector3& pa, const btVector3& ua,
+						  const btVector3& pb, const btVector3& ub,
+						  btScalar* alpha, btScalar* beta);
+void dLineClosestApproach(const btVector3& pa, const btVector3& ua,
+						  const btVector3& pb, const btVector3& ub,
+						  btScalar* alpha, btScalar* beta)
 {
-  btVector3 p;
-  p[0] = pb[0] - pa[0];
-  p[1] = pb[1] - pa[1];
-  p[2] = pb[2] - pa[2];
-  btScalar uaub = dDOT(ua,ub);
-  btScalar q1 =  dDOT(ua,p);
-  btScalar q2 = -dDOT(ub,p);
-  btScalar d = 1-uaub*uaub;
-  if (d <= btScalar(0.0001f)) {
-    // @@@ this needs to be made more robust
-    *alpha = 0;
-    *beta  = 0;
-  }
-  else {
-    d = 1.f/d;
-    *alpha = (q1 + uaub*q2)*d;
-    *beta  = (uaub*q1 + q2)*d;
-  }
+	btVector3 p;
+	p[0] = pb[0] - pa[0];
+	p[1] = pb[1] - pa[1];
+	p[2] = pb[2] - pa[2];
+	btScalar uaub = dDOT(ua, ub);
+	btScalar q1 = dDOT(ua, p);
+	btScalar q2 = -dDOT(ub, p);
+	btScalar d = 1 - uaub * uaub;
+	if (d <= btScalar(0.0001f))
+	{
+		// @@@ this needs to be made more robust
+		*alpha = 0;
+		*beta = 0;
+	}
+	else
+	{
+		d = 1.f / d;
+		*alpha = (q1 + uaub * q2) * d;
+		*beta = (uaub * q1 + q2) * d;
+	}
 }
 
-
-
 // find all the intersection points between the 2D rectangle with vertices
 // at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
 // (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
@@ -117,60 +114,66 @@ void dLineClosestApproach (const btVector3& pa, const btVector3& ua,
 // the number of intersection points is returned by the function (this will
 // be in the range 0 to 8).
 
-static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16])
+static int intersectRectQuad2(btScalar h[2], btScalar p[8], btScalar ret[16])
 {
-  // q (and r) contain nq (and nr) coordinate points for the current (and
-  // chopped) polygons
-  int nq=4,nr=0;
-  btScalar buffer[16];
-  btScalar *q = p;
-  btScalar *r = ret;
-  for (int dir=0; dir <= 1; dir++) {
-    // direction notation: xy[0] = x axis, xy[1] = y axis
-    for (int sign=-1; sign <= 1; sign += 2) {
-      // chop q along the line xy[dir] = sign*h[dir]
-      btScalar *pq = q;
-      btScalar *pr = r;
-      nr = 0;
-      for (int i=nq; i > 0; i--) {
-	// go through all points in q and all lines between adjacent points
-	if (sign*pq[dir] < h[dir]) {
-	  // this point is inside the chopping line
-	  pr[0] = pq[0];
-	  pr[1] = pq[1];
-	  pr += 2;
-	  nr++;
-	  if (nr & 8) {
-	    q = r;
-	    goto done;
-	  }
-	}
-	btScalar *nextq = (i > 1) ? pq+2 : q;
-	if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) {
-	  // this line crosses the chopping line
-	  pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
-	    (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
-	  pr[dir] = sign*h[dir];
-	  pr += 2;
-	  nr++;
-	  if (nr & 8) {
-	    q = r;
-	    goto done;
-	  }
+	// q (and r) contain nq (and nr) coordinate points for the current (and
+	// chopped) polygons
+	int nq = 4, nr = 0;
+	btScalar buffer[16];
+	btScalar* q = p;
+	btScalar* r = ret;
+	for (int dir = 0; dir <= 1; dir++)
+	{
+		// direction notation: xy[0] = x axis, xy[1] = y axis
+		for (int sign = -1; sign <= 1; sign += 2)
+		{
+			// chop q along the line xy[dir] = sign*h[dir]
+			btScalar* pq = q;
+			btScalar* pr = r;
+			nr = 0;
+			for (int i = nq; i > 0; i--)
+			{
+				// go through all points in q and all lines between adjacent points
+				if (sign * pq[dir] < h[dir])
+				{
+					// this point is inside the chopping line
+					pr[0] = pq[0];
+					pr[1] = pq[1];
+					pr += 2;
+					nr++;
+					if (nr & 8)
+					{
+						q = r;
+						goto done;
+					}
+				}
+				btScalar* nextq = (i > 1) ? pq + 2 : q;
+				if ((sign * pq[dir] < h[dir]) ^ (sign * nextq[dir] < h[dir]))
+				{
+					// this line crosses the chopping line
+					pr[1 - dir] = pq[1 - dir] + (nextq[1 - dir] - pq[1 - dir]) /
+													(nextq[dir] - pq[dir]) * (sign * h[dir] - pq[dir]);
+					pr[dir] = sign * h[dir];
+					pr += 2;
+					nr++;
+					if (nr & 8)
+					{
+						q = r;
+						goto done;
+					}
+				}
+				pq += 2;
+			}
+			q = r;
+			r = (q == ret) ? buffer : ret;
+			nq = nr;
+		}
 	}
-	pq += 2;
-      }
-      q = r;
-      r = (q==ret) ? buffer : ret;
-      nq = nr;
-    }
-  }
- done:
-  if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar));
-  return nr;
+done:
+	if (q != ret) memcpy(ret, q, nr * 2 * sizeof(btScalar));
+	return nr;
 }
 
-
 #define M__PI 3.14159265f
 
 // given n points in the plane (array p, of size 2*n), generate m points that
@@ -181,538 +184,584 @@ static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16])
 // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
 // in the range [0..n-1].
 
-void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]);
-void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[])
+void cullPoints2(int n, btScalar p[], int m, int i0, int iret[]);
+void cullPoints2(int n, btScalar p[], int m, int i0, int iret[])
 {
-  // compute the centroid of the polygon in cx,cy
-  int i,j;
-  btScalar a,cx,cy,q;
-  if (n==1) {
-    cx = p[0];
-    cy = p[1];
-  }
-  else if (n==2) {
-    cx = btScalar(0.5)*(p[0] + p[2]);
-    cy = btScalar(0.5)*(p[1] + p[3]);
-  }
-  else {
-    a = 0;
-    cx = 0;
-    cy = 0;
-    for (i=0; i<(n-1); i++) {
-      q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
-      a += q;
-      cx += q*(p[i*2]+p[i*2+2]);
-      cy += q*(p[i*2+1]+p[i*2+3]);
-    }
-    q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
-	if (btFabs(a+q) > SIMD_EPSILON)
+	// compute the centroid of the polygon in cx,cy
+	int i, j;
+	btScalar a, cx, cy, q;
+	if (n == 1)
 	{
-		a = 1.f/(btScalar(3.0)*(a+q));
-	} else
+		cx = p[0];
+		cy = p[1];
+	}
+	else if (n == 2)
 	{
-		a=BT_LARGE_FLOAT;
+		cx = btScalar(0.5) * (p[0] + p[2]);
+		cy = btScalar(0.5) * (p[1] + p[3]);
 	}
-    cx = a*(cx + q*(p[n*2-2]+p[0]));
-    cy = a*(cy + q*(p[n*2-1]+p[1]));
-  }
-
-  // compute the angle of each point w.r.t. the centroid
-  btScalar A[8];
-  for (i=0; i<n; i++) A[i] = btAtan2(p[i*2+1]-cy,p[i*2]-cx);
-
-  // search for points that have angles closest to A[i0] + i*(2*pi/m).
-  int avail[8];
-  for (i=0; i<n; i++) avail[i] = 1;
-  avail[i0] = 0;
-  iret[0] = i0;
-  iret++;
-  for (j=1; j<m; j++) {
-    a = btScalar(j)*(2*M__PI/m) + A[i0];
-    if (a > M__PI) a -= 2*M__PI;
-    btScalar maxdiff=1e9,diff;
-
-    *iret = i0;			// iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
-
-    for (i=0; i<n; i++) {
-      if (avail[i]) {
-	diff = btFabs (A[i]-a);
-	if (diff > M__PI) diff = 2*M__PI - diff;
-	if (diff < maxdiff) {
-	  maxdiff = diff;
-	  *iret = i;
+	else
+	{
+		a = 0;
+		cx = 0;
+		cy = 0;
+		for (i = 0; i < (n - 1); i++)
+		{
+			q = p[i * 2] * p[i * 2 + 3] - p[i * 2 + 2] * p[i * 2 + 1];
+			a += q;
+			cx += q * (p[i * 2] + p[i * 2 + 2]);
+			cy += q * (p[i * 2 + 1] + p[i * 2 + 3]);
+		}
+		q = p[n * 2 - 2] * p[1] - p[0] * p[n * 2 - 1];
+		if (btFabs(a + q) > SIMD_EPSILON)
+		{
+			a = 1.f / (btScalar(3.0) * (a + q));
+		}
+		else
+		{
+			a = BT_LARGE_FLOAT;
+		}
+		cx = a * (cx + q * (p[n * 2 - 2] + p[0]));
+		cy = a * (cy + q * (p[n * 2 - 1] + p[1]));
 	}
-      }
-    }
-#if defined(DEBUG) || defined (_DEBUG)
-    btAssert (*iret != i0);	// ensure iret got set
+
+	// compute the angle of each point w.r.t. the centroid
+	btScalar A[8];
+	for (i = 0; i < n; i++) A[i] = btAtan2(p[i * 2 + 1] - cy, p[i * 2] - cx);
+
+	// search for points that have angles closest to A[i0] + i*(2*pi/m).
+	int avail[8];
+	for (i = 0; i < n; i++) avail[i] = 1;
+	avail[i0] = 0;
+	iret[0] = i0;
+	iret++;
+	for (j = 1; j < m; j++)
+	{
+		a = btScalar(j) * (2 * M__PI / m) + A[i0];
+		if (a > M__PI) a -= 2 * M__PI;
+		btScalar maxdiff = 1e9, diff;
+
+		*iret = i0;  // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
+
+		for (i = 0; i < n; i++)
+		{
+			if (avail[i])
+			{
+				diff = btFabs(A[i] - a);
+				if (diff > M__PI) diff = 2 * M__PI - diff;
+				if (diff < maxdiff)
+				{
+					maxdiff = diff;
+					*iret = i;
+				}
+			}
+		}
+#if defined(DEBUG) || defined(_DEBUG)
+		btAssert(*iret != i0);  // ensure iret got set
 #endif
-    avail[*iret] = 0;
-    iret++;
-  }
+		avail[*iret] = 0;
+		iret++;
+	}
 }
 
+int dBoxBox2(const btVector3& p1, const dMatrix3 R1,
+			 const btVector3& side1, const btVector3& p2,
+			 const dMatrix3 R2, const btVector3& side2,
+			 btVector3& normal, btScalar* depth, int* return_code,
+			 int maxc, dContactGeom* /*contact*/, int /*skip*/, btDiscreteCollisionDetectorInterface::Result& output);
+int dBoxBox2(const btVector3& p1, const dMatrix3 R1,
+			 const btVector3& side1, const btVector3& p2,
+			 const dMatrix3 R2, const btVector3& side2,
+			 btVector3& normal, btScalar* depth, int* return_code,
+			 int maxc, dContactGeom* /*contact*/, int /*skip*/, btDiscreteCollisionDetectorInterface::Result& output)
+{
+	const btScalar fudge_factor = btScalar(1.05);
+	btVector3 p, pp, normalC(0.f, 0.f, 0.f);
+	const btScalar* normalR = 0;
+	btScalar A[3], B[3], R11, R12, R13, R21, R22, R23, R31, R32, R33,
+		Q11, Q12, Q13, Q21, Q22, Q23, Q31, Q32, Q33, s, s2, l;
+	int i, j, invert_normal, code;
+
+	// get vector from centers of box 1 to box 2, relative to box 1
+	p = p2 - p1;
+	dMULTIPLY1_331(pp, R1, p);  // get pp = p relative to body 1
+
+	// get side lengths / 2
+	A[0] = side1[0] * btScalar(0.5);
+	A[1] = side1[1] * btScalar(0.5);
+	A[2] = side1[2] * btScalar(0.5);
+	B[0] = side2[0] * btScalar(0.5);
+	B[1] = side2[1] * btScalar(0.5);
+	B[2] = side2[2] * btScalar(0.5);
+
+	// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
+	R11 = dDOT44(R1 + 0, R2 + 0);
+	R12 = dDOT44(R1 + 0, R2 + 1);
+	R13 = dDOT44(R1 + 0, R2 + 2);
+	R21 = dDOT44(R1 + 1, R2 + 0);
+	R22 = dDOT44(R1 + 1, R2 + 1);
+	R23 = dDOT44(R1 + 1, R2 + 2);
+	R31 = dDOT44(R1 + 2, R2 + 0);
+	R32 = dDOT44(R1 + 2, R2 + 1);
+	R33 = dDOT44(R1 + 2, R2 + 2);
+
+	Q11 = btFabs(R11);
+	Q12 = btFabs(R12);
+	Q13 = btFabs(R13);
+	Q21 = btFabs(R21);
+	Q22 = btFabs(R22);
+	Q23 = btFabs(R23);
+	Q31 = btFabs(R31);
+	Q32 = btFabs(R32);
+	Q33 = btFabs(R33);
+
+	// for all 15 possible separating axes:
+	//   * see if the axis separates the boxes. if so, return 0.
+	//   * find the depth of the penetration along the separating axis (s2)
+	//   * if this is the largest depth so far, record it.
+	// the normal vector will be set to the separating axis with the smallest
+	// depth. note: normalR is set to point to a column of R1 or R2 if that is
+	// the smallest depth normal so far. otherwise normalR is 0 and normalC is
+	// set to a vector relative to body 1. invert_normal is 1 if the sign of
+	// the normal should be flipped.
+
+#define TST(expr1, expr2, norm, cc)    \
+	s2 = btFabs(expr1) - (expr2);      \
+	if (s2 > 0) return 0;              \
+	if (s2 > s)                        \
+	{                                  \
+		s = s2;                        \
+		normalR = norm;                \
+		invert_normal = ((expr1) < 0); \
+		code = (cc);                   \
+	}
 
+	s = -dInfinity;
+	invert_normal = 0;
+	code = 0;
 
-int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
-	     const btVector3& side1, const btVector3& p2,
-	     const dMatrix3 R2, const btVector3& side2,
-	     btVector3& normal, btScalar *depth, int *return_code,
-		 int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output);
-int dBoxBox2 (const btVector3& p1, const dMatrix3 R1,
-	     const btVector3& side1, const btVector3& p2,
-	     const dMatrix3 R2, const btVector3& side2,
-	     btVector3& normal, btScalar *depth, int *return_code,
-		 int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output)
-{
-  const btScalar fudge_factor = btScalar(1.05);
-  btVector3 p,pp,normalC(0.f,0.f,0.f);
-  const btScalar *normalR = 0;
-  btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
-    Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
-  int i,j,invert_normal,code;
-
-  // get vector from centers of box 1 to box 2, relative to box 1
-  p = p2 - p1;
-  dMULTIPLY1_331 (pp,R1,p);		// get pp = p relative to body 1
-
-  // get side lengths / 2
-  A[0] = side1[0]*btScalar(0.5);
-  A[1] = side1[1]*btScalar(0.5);
-  A[2] = side1[2]*btScalar(0.5);
-  B[0] = side2[0]*btScalar(0.5);
-  B[1] = side2[1]*btScalar(0.5);
-  B[2] = side2[2]*btScalar(0.5);
-
-  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
-  R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
-  R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
-  R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
-
-  Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13);
-  Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23);
-  Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33);
-
-  // for all 15 possible separating axes:
-  //   * see if the axis separates the boxes. if so, return 0.
-  //   * find the depth of the penetration along the separating axis (s2)
-  //   * if this is the largest depth so far, record it.
-  // the normal vector will be set to the separating axis with the smallest
-  // depth. note: normalR is set to point to a column of R1 or R2 if that is
-  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
-  // set to a vector relative to body 1. invert_normal is 1 if the sign of
-  // the normal should be flipped.
-
-#define TST(expr1,expr2,norm,cc) \
-  s2 = btFabs(expr1) - (expr2); \
-  if (s2 > 0) return 0; \
-  if (s2 > s) { \
-    s = s2; \
-    normalR = norm; \
-    invert_normal = ((expr1) < 0); \
-    code = (cc); \
-  }
-
-  s = -dInfinity;
-  invert_normal = 0;
-  code = 0;
-
-  // separating axis = u1,u2,u3
-  TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
-  TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
-  TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
-
-  // separating axis = v1,v2,v3
-  TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
-  TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
-  TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
-
-  // note: cross product axes need to be scaled when s is computed.
-  // normal (n1,n2,n3) is relative to box 1.
+	// separating axis = u1,u2,u3
+	TST(pp[0], (A[0] + B[0] * Q11 + B[1] * Q12 + B[2] * Q13), R1 + 0, 1);
+	TST(pp[1], (A[1] + B[0] * Q21 + B[1] * Q22 + B[2] * Q23), R1 + 1, 2);
+	TST(pp[2], (A[2] + B[0] * Q31 + B[1] * Q32 + B[2] * Q33), R1 + 2, 3);
+
+	// separating axis = v1,v2,v3
+	TST(dDOT41(R2 + 0, p), (A[0] * Q11 + A[1] * Q21 + A[2] * Q31 + B[0]), R2 + 0, 4);
+	TST(dDOT41(R2 + 1, p), (A[0] * Q12 + A[1] * Q22 + A[2] * Q32 + B[1]), R2 + 1, 5);
+	TST(dDOT41(R2 + 2, p), (A[0] * Q13 + A[1] * Q23 + A[2] * Q33 + B[2]), R2 + 2, 6);
+
+	// note: cross product axes need to be scaled when s is computed.
+	// normal (n1,n2,n3) is relative to box 1.
 #undef TST
-#define TST(expr1,expr2,n1,n2,n3,cc) \
-  s2 = btFabs(expr1) - (expr2); \
-  if (s2 > SIMD_EPSILON) return 0; \
-  l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
-  if (l > SIMD_EPSILON) { \
-    s2 /= l; \
-    if (s2*fudge_factor > s) { \
-      s = s2; \
-      normalR = 0; \
-      normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
-      invert_normal = ((expr1) < 0); \
-      code = (cc); \
-    } \
-  }
-
-  btScalar fudge2 (1.0e-5f);
-
-  Q11 += fudge2;
-  Q12 += fudge2;
-  Q13 += fudge2;
-
-  Q21 += fudge2;
-  Q22 += fudge2;
-  Q23 += fudge2;
-
-  Q31 += fudge2;
-  Q32 += fudge2;
-  Q33 += fudge2;
-
-  // separating axis = u1 x (v1,v2,v3)
-  TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
-  TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
-  TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
-
-  // separating axis = u2 x (v1,v2,v3)
-  TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
-  TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
-  TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
-
-  // separating axis = u3 x (v1,v2,v3)
-  TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
-  TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
-  TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
+#define TST(expr1, expr2, n1, n2, n3, cc)                \
+	s2 = btFabs(expr1) - (expr2);                        \
+	if (s2 > SIMD_EPSILON) return 0;                     \
+	l = btSqrt((n1) * (n1) + (n2) * (n2) + (n3) * (n3)); \
+	if (l > SIMD_EPSILON)                                \
+	{                                                    \
+		s2 /= l;                                         \
+		if (s2 * fudge_factor > s)                       \
+		{                                                \
+			s = s2;                                      \
+			normalR = 0;                                 \
+			normalC[0] = (n1) / l;                       \
+			normalC[1] = (n2) / l;                       \
+			normalC[2] = (n3) / l;                       \
+			invert_normal = ((expr1) < 0);               \
+			code = (cc);                                 \
+		}                                                \
+	}
+
+	btScalar fudge2(1.0e-5f);
+
+	Q11 += fudge2;
+	Q12 += fudge2;
+	Q13 += fudge2;
+
+	Q21 += fudge2;
+	Q22 += fudge2;
+	Q23 += fudge2;
+
+	Q31 += fudge2;
+	Q32 += fudge2;
+	Q33 += fudge2;
+
+	// separating axis = u1 x (v1,v2,v3)
+	TST(pp[2] * R21 - pp[1] * R31, (A[1] * Q31 + A[2] * Q21 + B[1] * Q13 + B[2] * Q12), 0, -R31, R21, 7);
+	TST(pp[2] * R22 - pp[1] * R32, (A[1] * Q32 + A[2] * Q22 + B[0] * Q13 + B[2] * Q11), 0, -R32, R22, 8);
+	TST(pp[2] * R23 - pp[1] * R33, (A[1] * Q33 + A[2] * Q23 + B[0] * Q12 + B[1] * Q11), 0, -R33, R23, 9);
+
+	// separating axis = u2 x (v1,v2,v3)
+	TST(pp[0] * R31 - pp[2] * R11, (A[0] * Q31 + A[2] * Q11 + B[1] * Q23 + B[2] * Q22), R31, 0, -R11, 10);
+	TST(pp[0] * R32 - pp[2] * R12, (A[0] * Q32 + A[2] * Q12 + B[0] * Q23 + B[2] * Q21), R32, 0, -R12, 11);
+	TST(pp[0] * R33 - pp[2] * R13, (A[0] * Q33 + A[2] * Q13 + B[0] * Q22 + B[1] * Q21), R33, 0, -R13, 12);
+
+	// separating axis = u3 x (v1,v2,v3)
+	TST(pp[1] * R11 - pp[0] * R21, (A[0] * Q21 + A[1] * Q11 + B[1] * Q33 + B[2] * Q32), -R21, R11, 0, 13);
+	TST(pp[1] * R12 - pp[0] * R22, (A[0] * Q22 + A[1] * Q12 + B[0] * Q33 + B[2] * Q31), -R22, R12, 0, 14);
+	TST(pp[1] * R13 - pp[0] * R23, (A[0] * Q23 + A[1] * Q13 + B[0] * Q32 + B[1] * Q31), -R23, R13, 0, 15);
 
 #undef TST
 
-  if (!code) return 0;
-
-  // if we get to this point, the boxes interpenetrate. compute the normal
-  // in global coordinates.
-  if (normalR) {
-    normal[0] = normalR[0];
-    normal[1] = normalR[4];
-    normal[2] = normalR[8];
-  }
-  else {
-    dMULTIPLY0_331 (normal,R1,normalC);
-  }
-  if (invert_normal) {
-    normal[0] = -normal[0];
-    normal[1] = -normal[1];
-    normal[2] = -normal[2];
-  }
-  *depth = -s;
-
-  // compute contact point(s)
-
-  if (code > 6) {
-    // an edge from box 1 touches an edge from box 2.
-    // find a point pa on the intersecting edge of box 1
-    btVector3 pa;
-    btScalar sign;
-    for (i=0; i<3; i++) pa[i] = p1[i];
-    for (j=0; j<3; j++) {
-      sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0);
-      for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
-    }
-
-    // find a point pb on the intersecting edge of box 2
-    btVector3 pb;
-    for (i=0; i<3; i++) pb[i] = p2[i];
-    for (j=0; j<3; j++) {
-      sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0);
-      for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
-    }
-
-    btScalar alpha,beta;
-    btVector3 ua,ub;
-    for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
-    for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
-
-    dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
-    for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
-    for (i=0; i<3; i++) pb[i] += ub[i]*beta;
+	if (!code) return 0;
 
+	// if we get to this point, the boxes interpenetrate. compute the normal
+	// in global coordinates.
+	if (normalR)
+	{
+		normal[0] = normalR[0];
+		normal[1] = normalR[4];
+		normal[2] = normalR[8];
+	}
+	else
 	{
-		
-		//contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
-		//contact[0].depth = *depth;
-		btVector3 pointInWorld;
+		dMULTIPLY0_331(normal, R1, normalC);
+	}
+	if (invert_normal)
+	{
+		normal[0] = -normal[0];
+		normal[1] = -normal[1];
+		normal[2] = -normal[2];
+	}
+	*depth = -s;
+
+	// compute contact point(s)
+
+	if (code > 6)
+	{
+		// an edge from box 1 touches an edge from box 2.
+		// find a point pa on the intersecting edge of box 1
+		btVector3 pa;
+		btScalar sign;
+		for (i = 0; i < 3; i++) pa[i] = p1[i];
+		for (j = 0; j < 3; j++)
+		{
+			sign = (dDOT14(normal, R1 + j) > 0) ? btScalar(1.0) : btScalar(-1.0);
+			for (i = 0; i < 3; i++) pa[i] += sign * A[j] * R1[i * 4 + j];
+		}
+
+		// find a point pb on the intersecting edge of box 2
+		btVector3 pb;
+		for (i = 0; i < 3; i++) pb[i] = p2[i];
+		for (j = 0; j < 3; j++)
+		{
+			sign = (dDOT14(normal, R2 + j) > 0) ? btScalar(-1.0) : btScalar(1.0);
+			for (i = 0; i < 3; i++) pb[i] += sign * B[j] * R2[i * 4 + j];
+		}
+
+		btScalar alpha, beta;
+		btVector3 ua, ub;
+		for (i = 0; i < 3; i++) ua[i] = R1[((code)-7) / 3 + i * 4];
+		for (i = 0; i < 3; i++) ub[i] = R2[((code)-7) % 3 + i * 4];
+
+		dLineClosestApproach(pa, ua, pb, ub, &alpha, &beta);
+		for (i = 0; i < 3; i++) pa[i] += ua[i] * alpha;
+		for (i = 0; i < 3; i++) pb[i] += ub[i] * beta;
+
+		{
+			//contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
+			//contact[0].depth = *depth;
+			btVector3 pointInWorld;
 
 #ifdef USE_CENTER_POINT
-	    for (i=0; i<3; i++) 
-			pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5);
-		output.addContactPoint(-normal,pointInWorld,-*depth);
+			for (i = 0; i < 3; i++)
+				pointInWorld[i] = (pa[i] + pb[i]) * btScalar(0.5);
+			output.addContactPoint(-normal, pointInWorld, -*depth);
 #else
-		output.addContactPoint(-normal,pb,-*depth);
+			output.addContactPoint(-normal, pb, -*depth);
 
-#endif //
-		*return_code = code;
+#endif  //
+			*return_code = code;
+		}
+		return 1;
+	}
+
+	// okay, we have a face-something intersection (because the separating
+	// axis is perpendicular to a face). define face 'a' to be the reference
+	// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
+	// the incident face (the closest face of the other box).
+
+	const btScalar *Ra, *Rb, *pa, *pb, *Sa, *Sb;
+	if (code <= 3)
+	{
+		Ra = R1;
+		Rb = R2;
+		pa = p1;
+		pb = p2;
+		Sa = A;
+		Sb = B;
+	}
+	else
+	{
+		Ra = R2;
+		Rb = R1;
+		pa = p2;
+		pb = p1;
+		Sa = B;
+		Sb = A;
+	}
+
+	// nr = normal vector of reference face dotted with axes of incident box.
+	// anr = absolute values of nr.
+	btVector3 normal2, nr, anr;
+	if (code <= 3)
+	{
+		normal2[0] = normal[0];
+		normal2[1] = normal[1];
+		normal2[2] = normal[2];
+	}
+	else
+	{
+		normal2[0] = -normal[0];
+		normal2[1] = -normal[1];
+		normal2[2] = -normal[2];
 	}
-    return 1;
-  }
-
-  // okay, we have a face-something intersection (because the separating
-  // axis is perpendicular to a face). define face 'a' to be the reference
-  // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
-  // the incident face (the closest face of the other box).
-
-  const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb;
-  if (code <= 3) {
-    Ra = R1;
-    Rb = R2;
-    pa = p1;
-    pb = p2;
-    Sa = A;
-    Sb = B;
-  }
-  else {
-    Ra = R2;
-    Rb = R1;
-    pa = p2;
-    pb = p1;
-    Sa = B;
-    Sb = A;
-  }
-
-  // nr = normal vector of reference face dotted with axes of incident box.
-  // anr = absolute values of nr.
-  btVector3 normal2,nr,anr;
-  if (code <= 3) {
-    normal2[0] = normal[0];
-    normal2[1] = normal[1];
-    normal2[2] = normal[2];
-  }
-  else {
-    normal2[0] = -normal[0];
-    normal2[1] = -normal[1];
-    normal2[2] = -normal[2];
-  }
-  dMULTIPLY1_331 (nr,Rb,normal2);
-  anr[0] = btFabs (nr[0]);
-  anr[1] = btFabs (nr[1]);
-  anr[2] = btFabs (nr[2]);
-
-  // find the largest compontent of anr: this corresponds to the normal
-  // for the indident face. the other axis numbers of the indicent face
-  // are stored in a1,a2.
-  int lanr,a1,a2;
-  if (anr[1] > anr[0]) {
-    if (anr[1] > anr[2]) {
-      a1 = 0;
-      lanr = 1;
-      a2 = 2;
-    }
-    else {
-      a1 = 0;
-      a2 = 1;
-      lanr = 2;
-    }
-  }
-  else {
-    if (anr[0] > anr[2]) {
-      lanr = 0;
-      a1 = 1;
-      a2 = 2;
-    }
-    else {
-      a1 = 0;
-      a2 = 1;
-      lanr = 2;
-    }
-  }
-
-  // compute center point of incident face, in reference-face coordinates
-  btVector3 center;
-  if (nr[lanr] < 0) {
-    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
-  }
-  else {
-    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
-  }
-
-  // find the normal and non-normal axis numbers of the reference box
-  int codeN,code1,code2;
-  if (code <= 3) codeN = code-1; else codeN = code-4;
-  if (codeN==0) {
-    code1 = 1;
-    code2 = 2;
-  }
-  else if (codeN==1) {
-    code1 = 0;
-    code2 = 2;
-  }
-  else {
-    code1 = 0;
-    code2 = 1;
-  }
-
-  // find the four corners of the incident face, in reference-face coordinates
-  btScalar quad[8];	// 2D coordinate of incident face (x,y pairs)
-  btScalar c1,c2,m11,m12,m21,m22;
-  c1 = dDOT14 (center,Ra+code1);
-  c2 = dDOT14 (center,Ra+code2);
-  // optimize this? - we have already computed this data above, but it is not
-  // stored in an easy-to-index format. for now it's quicker just to recompute
-  // the four dot products.
-  m11 = dDOT44 (Ra+code1,Rb+a1);
-  m12 = dDOT44 (Ra+code1,Rb+a2);
-  m21 = dDOT44 (Ra+code2,Rb+a1);
-  m22 = dDOT44 (Ra+code2,Rb+a2);
-  {
-    btScalar k1 = m11*Sb[a1];
-    btScalar k2 = m21*Sb[a1];
-    btScalar k3 = m12*Sb[a2];
-    btScalar k4 = m22*Sb[a2];
-    quad[0] = c1 - k1 - k3;
-    quad[1] = c2 - k2 - k4;
-    quad[2] = c1 - k1 + k3;
-    quad[3] = c2 - k2 + k4;
-    quad[4] = c1 + k1 + k3;
-    quad[5] = c2 + k2 + k4;
-    quad[6] = c1 + k1 - k3;
-    quad[7] = c2 + k2 - k4;
-  }
-
-  // find the size of the reference face
-  btScalar rect[2];
-  rect[0] = Sa[code1];
-  rect[1] = Sa[code2];
-
-  // intersect the incident and reference faces
-  btScalar ret[16];
-  int n = intersectRectQuad2 (rect,quad,ret);
-  if (n < 1) return 0;		// this should never happen
-
-  // convert the intersection points into reference-face coordinates,
-  // and compute the contact position and depth for each point. only keep
-  // those points that have a positive (penetrating) depth. delete points in
-  // the 'ret' array as necessary so that 'point' and 'ret' correspond.
-  btScalar point[3*8];		// penetrating contact points
-  btScalar dep[8];			// depths for those points
-  btScalar det1 = 1.f/(m11*m22 - m12*m21);
-  m11 *= det1;
-  m12 *= det1;
-  m21 *= det1;
-  m22 *= det1;
-  int cnum = 0;			// number of penetrating contact points found
-  for (j=0; j < n; j++) {
-    btScalar k1 =  m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
-    btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
-    for (i=0; i<3; i++) point[cnum*3+i] =
-			  center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
-    dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
-    if (dep[cnum] >= 0) {
-      ret[cnum*2] = ret[j*2];
-      ret[cnum*2+1] = ret[j*2+1];
-      cnum++;
-    }
-  }
-  if (cnum < 1) return 0;	// this should never happen
-
-  // we can't generate more contacts than we actually have
-  if (maxc > cnum) maxc = cnum;
-  if (maxc < 1) maxc = 1;
-
-  if (cnum <= maxc) {
-
-	  if (code<4) 
-	  {
-    // we have less contacts than we need, so we use them all
-    for (j=0; j < cnum; j++) 
+	dMULTIPLY1_331(nr, Rb, normal2);
+	anr[0] = btFabs(nr[0]);
+	anr[1] = btFabs(nr[1]);
+	anr[2] = btFabs(nr[2]);
+
+	// find the largest compontent of anr: this corresponds to the normal
+	// for the indident face. the other axis numbers of the indicent face
+	// are stored in a1,a2.
+	int lanr, a1, a2;
+	if (anr[1] > anr[0])
 	{
-		btVector3 pointInWorld;
-		for (i=0; i<3; i++) 
-			pointInWorld[i] = point[j*3+i] + pa[i];
-		output.addContactPoint(-normal,pointInWorld,-dep[j]);
-
-    }
-	  } else
-	  {
-		  // we have less contacts than we need, so we use them all
-		for (j=0; j < cnum; j++) 
+		if (anr[1] > anr[2])
 		{
-			btVector3 pointInWorld;
-			for (i=0; i<3; i++) 
-				pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j];
+			a1 = 0;
+			lanr = 1;
+			a2 = 2;
+		}
+		else
+		{
+			a1 = 0;
+			a2 = 1;
+			lanr = 2;
+		}
+	}
+	else
+	{
+		if (anr[0] > anr[2])
+		{
+			lanr = 0;
+			a1 = 1;
+			a2 = 2;
+		}
+		else
+		{
+			a1 = 0;
+			a2 = 1;
+			lanr = 2;
+		}
+	}
+
+	// compute center point of incident face, in reference-face coordinates
+	btVector3 center;
+	if (nr[lanr] < 0)
+	{
+		for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i * 4 + lanr];
+	}
+	else
+	{
+		for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i * 4 + lanr];
+	}
+
+	// find the normal and non-normal axis numbers of the reference box
+	int codeN, code1, code2;
+	if (code <= 3)
+		codeN = code - 1;
+	else
+		codeN = code - 4;
+	if (codeN == 0)
+	{
+		code1 = 1;
+		code2 = 2;
+	}
+	else if (codeN == 1)
+	{
+		code1 = 0;
+		code2 = 2;
+	}
+	else
+	{
+		code1 = 0;
+		code2 = 1;
+	}
+
+	// find the four corners of the incident face, in reference-face coordinates
+	btScalar quad[8];  // 2D coordinate of incident face (x,y pairs)
+	btScalar c1, c2, m11, m12, m21, m22;
+	c1 = dDOT14(center, Ra + code1);
+	c2 = dDOT14(center, Ra + code2);
+	// optimize this? - we have already computed this data above, but it is not
+	// stored in an easy-to-index format. for now it's quicker just to recompute
+	// the four dot products.
+	m11 = dDOT44(Ra + code1, Rb + a1);
+	m12 = dDOT44(Ra + code1, Rb + a2);
+	m21 = dDOT44(Ra + code2, Rb + a1);
+	m22 = dDOT44(Ra + code2, Rb + a2);
+	{
+		btScalar k1 = m11 * Sb[a1];
+		btScalar k2 = m21 * Sb[a1];
+		btScalar k3 = m12 * Sb[a2];
+		btScalar k4 = m22 * Sb[a2];
+		quad[0] = c1 - k1 - k3;
+		quad[1] = c2 - k2 - k4;
+		quad[2] = c1 - k1 + k3;
+		quad[3] = c2 - k2 + k4;
+		quad[4] = c1 + k1 + k3;
+		quad[5] = c2 + k2 + k4;
+		quad[6] = c1 + k1 - k3;
+		quad[7] = c2 + k2 - k4;
+	}
+
+	// find the size of the reference face
+	btScalar rect[2];
+	rect[0] = Sa[code1];
+	rect[1] = Sa[code2];
+
+	// intersect the incident and reference faces
+	btScalar ret[16];
+	int n = intersectRectQuad2(rect, quad, ret);
+	if (n < 1) return 0;  // this should never happen
+
+	// convert the intersection points into reference-face coordinates,
+	// and compute the contact position and depth for each point. only keep
+	// those points that have a positive (penetrating) depth. delete points in
+	// the 'ret' array as necessary so that 'point' and 'ret' correspond.
+	btScalar point[3 * 8];  // penetrating contact points
+	btScalar dep[8];        // depths for those points
+	btScalar det1 = 1.f / (m11 * m22 - m12 * m21);
+	m11 *= det1;
+	m12 *= det1;
+	m21 *= det1;
+	m22 *= det1;
+	int cnum = 0;  // number of penetrating contact points found
+	for (j = 0; j < n; j++)
+	{
+		btScalar k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2);
+		btScalar k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2);
+		for (i = 0; i < 3; i++) point[cnum * 3 + i] =
+									center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2];
+		dep[cnum] = Sa[codeN] - dDOT(normal2, point + cnum * 3);
+		if (dep[cnum] >= 0)
+		{
+			ret[cnum * 2] = ret[j * 2];
+			ret[cnum * 2 + 1] = ret[j * 2 + 1];
+			cnum++;
+		}
+	}
+	if (cnum < 1) return 0;  // this should never happen
+
+	// we can't generate more contacts than we actually have
+	if (maxc > cnum) maxc = cnum;
+	if (maxc < 1) maxc = 1;
+
+	if (cnum <= maxc)
+	{
+		if (code < 4)
+		{
+			// we have less contacts than we need, so we use them all
+			for (j = 0; j < cnum; j++)
+			{
+				btVector3 pointInWorld;
+				for (i = 0; i < 3; i++)
+					pointInWorld[i] = point[j * 3 + i] + pa[i];
+				output.addContactPoint(-normal, pointInWorld, -dep[j]);
+			}
+		}
+		else
+		{
+			// we have less contacts than we need, so we use them all
+			for (j = 0; j < cnum; j++)
+			{
+				btVector3 pointInWorld;
+				for (i = 0; i < 3; i++)
+					pointInWorld[i] = point[j * 3 + i] + pa[i] - normal[i] * dep[j];
 				//pointInWorld[i] = point[j*3+i] + pa[i];
-			output.addContactPoint(-normal,pointInWorld,-dep[j]);
+				output.addContactPoint(-normal, pointInWorld, -dep[j]);
+			}
 		}
-	  }
-  }
-  else {
-    // we have more contacts than are wanted, some of them must be culled.
-    // find the deepest point, it is always the first contact.
-    int i1 = 0;
-    btScalar maxdepth = dep[0];
-    for (i=1; i<cnum; i++) {
-      if (dep[i] > maxdepth) {
-	maxdepth = dep[i];
-	i1 = i;
-      }
-    }
-
-    int iret[8];
-    cullPoints2 (cnum,ret,maxc,i1,iret);
-
-    for (j=0; j < maxc; j++) {
-//      dContactGeom *con = CONTACT(contact,skip*j);
-  //    for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
-    //  con->depth = dep[iret[j]];
-
-		btVector3 posInWorld;
-		for (i=0; i<3; i++) 
-			posInWorld[i] = point[iret[j]*3+i] + pa[i];
-		if (code<4) 
-	   {
-			output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
-		} else
+	}
+	else
+	{
+		// we have more contacts than are wanted, some of them must be culled.
+		// find the deepest point, it is always the first contact.
+		int i1 = 0;
+		btScalar maxdepth = dep[0];
+		for (i = 1; i < cnum; i++)
 		{
-			output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]);
+			if (dep[i] > maxdepth)
+			{
+				maxdepth = dep[i];
+				i1 = i;
+			}
 		}
-    }
-    cnum = maxc;
-  }
 
-  *return_code = code;
-  return cnum;
+		int iret[8];
+		cullPoints2(cnum, ret, maxc, i1, iret);
+
+		for (j = 0; j < maxc; j++)
+		{
+			//      dContactGeom *con = CONTACT(contact,skip*j);
+			//    for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
+			//  con->depth = dep[iret[j]];
+
+			btVector3 posInWorld;
+			for (i = 0; i < 3; i++)
+				posInWorld[i] = point[iret[j] * 3 + i] + pa[i];
+			if (code < 4)
+			{
+				output.addContactPoint(-normal, posInWorld, -dep[iret[j]]);
+			}
+			else
+			{
+				output.addContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]);
+			}
+		}
+		cnum = maxc;
+	}
+
+	*return_code = code;
+	return cnum;
 }
 
-void	btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/)
+void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* /*debugDraw*/, bool /*swapResults*/)
 {
-	
 	const btTransform& transformA = input.m_transformA;
 	const btTransform& transformB = input.m_transformB;
-	
+
 	int skip = 0;
-	dContactGeom *contact = 0;
+	dContactGeom* contact = 0;
 
 	dMatrix3 R1;
 	dMatrix3 R2;
 
-	for (int j=0;j<3;j++)
+	for (int j = 0; j < 3; j++)
 	{
-		R1[0+4*j] = transformA.getBasis()[j].x();
-		R2[0+4*j] = transformB.getBasis()[j].x();
-
-		R1[1+4*j] = transformA.getBasis()[j].y();
-		R2[1+4*j] = transformB.getBasis()[j].y();
-
+		R1[0 + 4 * j] = transformA.getBasis()[j].x();
+		R2[0 + 4 * j] = transformB.getBasis()[j].x();
 
-		R1[2+4*j] = transformA.getBasis()[j].z();
-		R2[2+4*j] = transformB.getBasis()[j].z();
+		R1[1 + 4 * j] = transformA.getBasis()[j].y();
+		R2[1 + 4 * j] = transformB.getBasis()[j].y();
 
+		R1[2 + 4 * j] = transformA.getBasis()[j].z();
+		R2[2 + 4 * j] = transformB.getBasis()[j].z();
 	}
 
-	
-
 	btVector3 normal;
 	btScalar depth;
 	int return_code;
 	int maxc = 4;
 
-
-	dBoxBox2 (transformA.getOrigin(), 
-	R1,
-	2.f*m_box1->getHalfExtentsWithMargin(),
-	transformB.getOrigin(),
-	R2, 
-	2.f*m_box2->getHalfExtentsWithMargin(),
-	normal, &depth, &return_code,
-	maxc, contact, skip,
-	output
-	);
-
+	dBoxBox2(transformA.getOrigin(),
+			 R1,
+			 2.f * m_box1->getHalfExtentsWithMargin(),
+			 transformB.getOrigin(),
+			 R2,
+			 2.f * m_box2->getHalfExtentsWithMargin(),
+			 normal, &depth, &return_code,
+			 maxc, contact, skip,
+			 output);
 }