1 files changed, 166 insertions, 188 deletions

diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
index ea380bc5f1..9d1836037d 100644
--- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
@@ -13,45 +13,42 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 ///This file was written by Erwin Coumans
 ///Separating axis rest based on work from Pierre Terdiman, see
 ///And contact clipping based on work from Simon Hobbs
 
-
 #include "btPolyhedralContactClipping.h"
 #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
 
-#include <float.h> //for FLT_MAX
+#include <float.h>  //for FLT_MAX
 
-int gExpectedNbTests=0;
+int gExpectedNbTests = 0;
 int gActualNbTests = 0;
 bool gUseInternalObject = true;
 
 // Clips a face to the back of a plane
-void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS)
+void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS, btScalar planeEqWS)
 {
-	
 	int ve;
 	btScalar ds, de;
 	int numVerts = pVtxIn.size();
 	if (numVerts < 2)
 		return;
 
-	btVector3 firstVertex=pVtxIn[pVtxIn.size()-1];
+	btVector3 firstVertex = pVtxIn[pVtxIn.size() - 1];
 	btVector3 endVertex = pVtxIn[0];
-	
-	ds = planeNormalWS.dot(firstVertex)+planeEqWS;
+
+	ds = planeNormalWS.dot(firstVertex) + planeEqWS;
 
 	for (ve = 0; ve < numVerts; ve++)
 	{
-		endVertex=pVtxIn[ve];
+		endVertex = pVtxIn[ve];
 
-		de = planeNormalWS.dot(endVertex)+planeEqWS;
+		de = planeNormalWS.dot(endVertex) + planeEqWS;
 
-		if (ds<0)
+		if (ds < 0)
 		{
-			if (de<0)
+			if (de < 0)
 			{
 				// Start < 0, end < 0, so output endVertex
 				ppVtxOut.push_back(endVertex);
@@ -59,15 +56,15 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex
 			else
 			{
 				// Start < 0, end >= 0, so output intersection
-				ppVtxOut.push_back( 	firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
+				ppVtxOut.push_back(firstVertex.lerp(endVertex, btScalar(ds * 1.f / (ds - de))));
 			}
 		}
 		else
 		{
-			if (de<0)
+			if (de < 0)
 			{
 				// Start >= 0, end < 0 so output intersection and end
-				ppVtxOut.push_back(firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
+				ppVtxOut.push_back(firstVertex.lerp(endVertex, btScalar(ds * 1.f / (ds - de))));
 				ppVtxOut.push_back(endVertex);
 			}
 		}
@@ -76,47 +73,44 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex
 	}
 }
 
-
-static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB)
+static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB)
 {
-	btScalar Min0,Max0;
-	btScalar Min1,Max1;
-	btVector3 witnesPtMinA,witnesPtMaxA;
-	btVector3 witnesPtMinB,witnesPtMaxB;
+	btScalar Min0, Max0;
+	btScalar Min1, Max1;
+	btVector3 witnesPtMinA, witnesPtMaxA;
+	btVector3 witnesPtMinB, witnesPtMaxB;
 
-	hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA);
-	hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB);
+	hullA.project(transA, sep_axis, Min0, Max0, witnesPtMinA, witnesPtMaxA);
+	hullB.project(transB, sep_axis, Min1, Max1, witnesPtMinB, witnesPtMaxB);
 
-	if(Max0<Min1 || Max1<Min0)
+	if (Max0 < Min1 || Max1 < Min0)
 		return false;
 
 	btScalar d0 = Max0 - Min1;
-	btAssert(d0>=0.0f);
+	btAssert(d0 >= 0.0f);
 	btScalar d1 = Max1 - Min0;
-	btAssert(d1>=0.0f);
-	if (d0<d1)
+	btAssert(d1 >= 0.0f);
+	if (d0 < d1)
 	{
 		depth = d0;
 		witnessPointA = witnesPtMaxA;
 		witnessPointB = witnesPtMinB;
-
-	} else
+	}
+	else
 	{
 		depth = d1;
 		witnessPointA = witnesPtMinA;
 		witnessPointB = witnesPtMaxB;
 	}
-	
+
 	return true;
 }
 
-
-
-static int gActualSATPairTests=0;
+static int gActualSATPairTests = 0;
 
 inline bool IsAlmostZero(const btVector3& v)
 {
-	if(btFabs(v.x())>1e-6 || btFabs(v.y())>1e-6 || btFabs(v.z())>1e-6)	return false;
+	if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false;
 	return true;
 }
 
@@ -125,9 +119,9 @@ inline bool IsAlmostZero(const btVector3& v)
 inline void BoxSupport(const btScalar extents[3], const btScalar sv[3], btScalar p[3])
 {
 	// This version is ~11.000 cycles (4%) faster overall in one of the tests.
-//	IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK);
-//	IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK);
-//	IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK);
+	//	IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK);
+	//	IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK);
+	//	IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK);
 	p[0] = sv[0] < 0.0f ? -extents[0] : extents[0];
 	p[1] = sv[1] < 0.0f ? -extents[1] : extents[1];
 	p[2] = sv[2] < 0.0f ? -extents[2] : extents[2];
@@ -140,90 +134,94 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans
 	const btVector3& r1 = rot[1];
 	const btVector3& r2 = rot[2];
 
-	const btScalar x = r0.x()*in.x() + r1.x()*in.y() + r2.x()*in.z();
-	const btScalar y = r0.y()*in.x() + r1.y()*in.y() + r2.y()*in.z();
-	const btScalar z = r0.z()*in.x() + r1.z()*in.y() + r2.z()*in.z();
+	const btScalar x = r0.x() * in.x() + r1.x() * in.y() + r2.x() * in.z();
+	const btScalar y = r0.y() * in.x() + r1.y() * in.y() + r2.y() * in.z();
+	const btScalar z = r0.z() * in.x() + r1.z() * in.y() + r2.z() * in.z();
 
 	out.setValue(x, y, z);
 }
 
- bool TestInternalObjects( const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin)
+bool TestInternalObjects(const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin)
 {
 	const btScalar dp = delta_c.dot(axis);
 
 	btVector3 localAxis0;
-	InverseTransformPoint3x3(localAxis0, axis,trans0);
+	InverseTransformPoint3x3(localAxis0, axis, trans0);
 	btVector3 localAxis1;
-	InverseTransformPoint3x3(localAxis1, axis,trans1);
+	InverseTransformPoint3x3(localAxis1, axis, trans1);
 
 	btScalar p0[3];
 	BoxSupport(convex0.m_extents, localAxis0, p0);
 	btScalar p1[3];
 	BoxSupport(convex1.m_extents, localAxis1, p1);
 
-	const btScalar Radius0 = p0[0]*localAxis0.x() + p0[1]*localAxis0.y() + p0[2]*localAxis0.z();
-	const btScalar Radius1 = p1[0]*localAxis1.x() + p1[1]*localAxis1.y() + p1[2]*localAxis1.z();
+	const btScalar Radius0 = p0[0] * localAxis0.x() + p0[1] * localAxis0.y() + p0[2] * localAxis0.z();
+	const btScalar Radius1 = p1[0] * localAxis1.x() + p1[1] * localAxis1.y() + p1[2] * localAxis1.z();
 
-	const btScalar MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius;
-	const btScalar MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius;
+	const btScalar MinRadius = Radius0 > convex0.m_radius ? Radius0 : convex0.m_radius;
+	const btScalar MaxRadius = Radius1 > convex1.m_radius ? Radius1 : convex1.m_radius;
 
 	const btScalar MinMaxRadius = MaxRadius + MinRadius;
 	const btScalar d0 = MinMaxRadius + dp;
 	const btScalar d1 = MinMaxRadius - dp;
 
-	const btScalar depth = d0<d1 ? d0:d1;
-	if(depth>dmin)
+	const btScalar depth = d0 < d1 ? d0 : d1;
+	if (depth > dmin)
 		return false;
 	return true;
 }
-#endif //TEST_INTERNAL_OBJECTS
+#endif  //TEST_INTERNAL_OBJECTS
 
- 
- 
- SIMD_FORCE_INLINE void btSegmentsClosestPoints(
+SIMD_FORCE_INLINE void btSegmentsClosestPoints(
 	btVector3& ptsVector,
 	btVector3& offsetA,
 	btVector3& offsetB,
 	btScalar& tA, btScalar& tB,
 	const btVector3& translation,
 	const btVector3& dirA, btScalar hlenA,
-	const btVector3& dirB, btScalar hlenB )
+	const btVector3& dirB, btScalar hlenB)
 {
 	// compute the parameters of the closest points on each line segment
 
-	btScalar dirA_dot_dirB = btDot(dirA,dirB);
-	btScalar dirA_dot_trans = btDot(dirA,translation);
-	btScalar dirB_dot_trans = btDot(dirB,translation);
+	btScalar dirA_dot_dirB = btDot(dirA, dirB);
+	btScalar dirA_dot_trans = btDot(dirA, translation);
+	btScalar dirB_dot_trans = btDot(dirB, translation);
 
 	btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
 
-	if ( denom == 0.0f ) {
+	if (denom == 0.0f)
+	{
 		tA = 0.0f;
-	} else {
-		tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
-		if ( tA < -hlenA )
+	}
+	else
+	{
+		tA = (dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB) / denom;
+		if (tA < -hlenA)
 			tA = -hlenA;
-		else if ( tA > hlenA )
+		else if (tA > hlenA)
 			tA = hlenA;
 	}
 
 	tB = tA * dirA_dot_dirB - dirB_dot_trans;
 
-	if ( tB < -hlenB ) {
+	if (tB < -hlenB)
+	{
 		tB = -hlenB;
 		tA = tB * dirA_dot_dirB + dirA_dot_trans;
 
-		if ( tA < -hlenA )
+		if (tA < -hlenA)
 			tA = -hlenA;
-		else if ( tA > hlenA )
+		else if (tA > hlenA)
 			tA = hlenA;
-	} else if ( tB > hlenB ) {
+	}
+	else if (tB > hlenB)
+	{
 		tB = hlenB;
 		tA = tB * dirA_dot_dirB + dirA_dot_trans;
 
-		if ( tA < -hlenA )
+		if (tA < -hlenA)
 			tA = -hlenA;
-		else if ( tA > hlenA )
+		else if (tA > hlenA)
 			tA = hlenA;
 	}
 
@@ -235,44 +233,42 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans
 	ptsVector = translation - offsetA + offsetB;
 }
 
-
-
-bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
+bool btPolyhedralContactClipping::findSeparatingAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
 {
 	gActualSATPairTests++;
 
-//#ifdef TEST_INTERNAL_OBJECTS
+	//#ifdef TEST_INTERNAL_OBJECTS
 	const btVector3 c0 = transA * hullA.m_localCenter;
 	const btVector3 c1 = transB * hullB.m_localCenter;
 	const btVector3 DeltaC2 = c0 - c1;
-//#endif
+	//#endif
 
 	btScalar dmin = FLT_MAX;
-	int curPlaneTests=0;
+	int curPlaneTests = 0;
 
 	int numFacesA = hullA.m_faces.size();
 	// Test normals from hullA
-	for(int i=0;i<numFacesA;i++)
+	for (int i = 0; i < numFacesA; i++)
 	{
 		const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
 		btVector3 faceANormalWS = transA.getBasis() * Normal;
-		if (DeltaC2.dot(faceANormalWS)<0)
-			faceANormalWS*=-1.f;
+		if (DeltaC2.dot(faceANormalWS) < 0)
+			faceANormalWS *= -1.f;
 
 		curPlaneTests++;
 #ifdef TEST_INTERNAL_OBJECTS
 		gExpectedNbTests++;
-		if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
+		if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
 			continue;
 		gActualNbTests++;
 #endif
 
 		btScalar d;
-		btVector3 wA,wB;
-		if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d,wA,wB))
+		btVector3 wA, wB;
+		if (!TestSepAxis(hullA, hullB, transA, transB, faceANormalWS, d, wA, wB))
 			return false;
 
-		if(d<dmin)
+		if (d < dmin)
 		{
 			dmin = d;
 			sep = faceANormalWS;
@@ -281,92 +277,89 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 
 	int numFacesB = hullB.m_faces.size();
 	// Test normals from hullB
-	for(int i=0;i<numFacesB;i++)
+	for (int i = 0; i < numFacesB; i++)
 	{
 		const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
 		btVector3 WorldNormal = transB.getBasis() * Normal;
-		if (DeltaC2.dot(WorldNormal)<0)
-			WorldNormal *=-1.f;
+		if (DeltaC2.dot(WorldNormal) < 0)
+			WorldNormal *= -1.f;
 
 		curPlaneTests++;
 #ifdef TEST_INTERNAL_OBJECTS
 		gExpectedNbTests++;
-		if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
+		if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
 			continue;
 		gActualNbTests++;
 #endif
 
 		btScalar d;
-		btVector3 wA,wB;
-		if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d,wA,wB))
+		btVector3 wA, wB;
+		if (!TestSepAxis(hullA, hullB, transA, transB, WorldNormal, d, wA, wB))
 			return false;
 
-		if(d<dmin)
+		if (d < dmin)
 		{
 			dmin = d;
 			sep = WorldNormal;
 		}
 	}
 
-	btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
-	int edgeA=-1;
-	int edgeB=-1;
+	btVector3 edgeAstart, edgeAend, edgeBstart, edgeBend;
+	int edgeA = -1;
+	int edgeB = -1;
 	btVector3 worldEdgeA;
 	btVector3 worldEdgeB;
-	btVector3 witnessPointA(0,0,0),witnessPointB(0,0,0);
-	
+	btVector3 witnessPointA(0, 0, 0), witnessPointB(0, 0, 0);
 
 	int curEdgeEdge = 0;
 	// Test edges
-	for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++)
+	for (int e0 = 0; e0 < hullA.m_uniqueEdges.size(); e0++)
 	{
 		const btVector3 edge0 = hullA.m_uniqueEdges[e0];
 		const btVector3 WorldEdge0 = transA.getBasis() * edge0;
-		for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++)
+		for (int e1 = 0; e1 < hullB.m_uniqueEdges.size(); e1++)
 		{
 			const btVector3 edge1 = hullB.m_uniqueEdges[e1];
 			const btVector3 WorldEdge1 = transB.getBasis() * edge1;
 
 			btVector3 Cross = WorldEdge0.cross(WorldEdge1);
 			curEdgeEdge++;
-			if(!IsAlmostZero(Cross))
+			if (!IsAlmostZero(Cross))
 			{
 				Cross = Cross.normalize();
-				if (DeltaC2.dot(Cross)<0)
+				if (DeltaC2.dot(Cross) < 0)
 					Cross *= -1.f;
 
-
 #ifdef TEST_INTERNAL_OBJECTS
 				gExpectedNbTests++;
-				if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
+				if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
 					continue;
 				gActualNbTests++;
 #endif
 
 				btScalar dist;
-				btVector3 wA,wB;
-				if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist,wA,wB))
+				btVector3 wA, wB;
+				if (!TestSepAxis(hullA, hullB, transA, transB, Cross, dist, wA, wB))
 					return false;
 
-				if(dist<dmin)
+				if (dist < dmin)
 				{
 					dmin = dist;
 					sep = Cross;
-					edgeA=e0;
-					edgeB=e1;
+					edgeA = e0;
+					edgeB = e1;
 					worldEdgeA = WorldEdge0;
 					worldEdgeB = WorldEdge1;
-					witnessPointA=wA;
-					witnessPointB=wB;
+					witnessPointA = wA;
+					witnessPointB = wB;
 				}
 			}
 		}
-
 	}
 
-	if (edgeA>=0&&edgeB>=0)
+	if (edgeA >= 0 && edgeB >= 0)
 	{
-//		printf("edge-edge\n");
+		//		printf("edge-edge\n");
 		//add an edge-edge contact
 
 		btVector3 ptsVector;
@@ -375,57 +368,55 @@ bool btPolyhedralContactClipping::findSeparatingAxis(	const btConvexPolyhedron&
 		btScalar tA;
 		btScalar tB;
 
-		btVector3 translation = witnessPointB-witnessPointA;
+		btVector3 translation = witnessPointB - witnessPointA;
 
 		btVector3 dirA = worldEdgeA;
 		btVector3 dirB = worldEdgeB;
-		
+
 		btScalar hlenB = 1e30f;
 		btScalar hlenA = 1e30f;
 
-		btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB,
-			translation,
-			dirA, hlenA,
-			dirB,hlenB);
+		btSegmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB,
+								translation,
+								dirA, hlenA,
+								dirB, hlenB);
 
 		btScalar nlSqrt = ptsVector.length2();
-		if (nlSqrt>SIMD_EPSILON)
+		if (nlSqrt > SIMD_EPSILON)
 		{
 			btScalar nl = btSqrt(nlSqrt);
-			ptsVector *= 1.f/nl;
-			if (ptsVector.dot(DeltaC2)<0.f)
+			ptsVector *= 1.f / nl;
+			if (ptsVector.dot(DeltaC2) < 0.f)
 			{
-				ptsVector*=-1.f;
+				ptsVector *= -1.f;
 			}
 			btVector3 ptOnB = witnessPointB + offsetB;
 			btScalar distance = nl;
-			resultOut.addContactPoint(ptsVector, ptOnB,-distance);
+			resultOut.addContactPoint(ptsVector, ptOnB, -distance);
 		}
-
 	}
 
-
-	if((DeltaC2.dot(sep))<0.0f)
+	if ((DeltaC2.dot(sep)) < 0.0f)
 		sep = -sep;
 
 	return true;
 }
 
-void	btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA,  const btTransform& transA, btVertexArray& worldVertsB1,btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
+void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut)
 {
 	worldVertsB2.resize(0);
 	btVertexArray* pVtxIn = &worldVertsB1;
 	btVertexArray* pVtxOut = &worldVertsB2;
 	pVtxOut->reserve(pVtxIn->size());
 
-	int closestFaceA=-1;
+	int closestFaceA = -1;
 	{
 		btScalar dmin = FLT_MAX;
-		for(int face=0;face<hullA.m_faces.size();face++)
+		for (int face = 0; face < hullA.m_faces.size(); face++)
 		{
 			const btVector3 Normal(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
 			const btVector3 faceANormalWS = transA.getBasis() * Normal;
-		
+
 			btScalar d = faceANormalWS.dot(separatingNormal);
 			if (d < dmin)
 			{
@@ -434,69 +425,66 @@ void	btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
 			}
 		}
 	}
-	if (closestFaceA<0)
+	if (closestFaceA < 0)
 		return;
 
 	const btFace& polyA = hullA.m_faces[closestFaceA];
 
-		// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
+	// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
 	int numVerticesA = polyA.m_indices.size();
-	for(int e0=0;e0<numVerticesA;e0++)
+	for (int e0 = 0; e0 < numVerticesA; e0++)
 	{
 		const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]];
-		const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]];
+		const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0 + 1) % numVerticesA]];
 		const btVector3 edge0 = a - b;
 		const btVector3 WorldEdge0 = transA.getBasis() * edge0;
-		btVector3 worldPlaneAnormal1 = transA.getBasis()* btVector3(polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
+		btVector3 worldPlaneAnormal1 = transA.getBasis() * btVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
 
-		btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);//.cross(WorldEdge0);
-		btVector3 worldA1 = transA*a;
+		btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);  //.cross(WorldEdge0);
+		btVector3 worldA1 = transA * a;
 		btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1);
-		
+
 //int otherFace=0;
 #ifdef BLA1
 		int otherFace = polyA.m_connectedFaces[e0];
-		btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
+		btVector3 localPlaneNormal(hullA.m_faces[otherFace].m_plane[0], hullA.m_faces[otherFace].m_plane[1], hullA.m_faces[otherFace].m_plane[2]);
 		btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
 
-		btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
-		btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
-#else 
+		btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal;
+		btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin());
+#else
 		btVector3 planeNormalWS = planeNormalWS1;
-		btScalar planeEqWS=planeEqWS1;
-		
+		btScalar planeEqWS = planeEqWS1;
+
 #endif
 		//clip face
 
-		clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
-		btSwap(pVtxIn,pVtxOut);
+		clipFace(*pVtxIn, *pVtxOut, planeNormalWS, planeEqWS);
+		btSwap(pVtxIn, pVtxOut);
 		pVtxOut->resize(0);
 	}
 
-
-
-//#define ONLY_REPORT_DEEPEST_POINT
+	//#define ONLY_REPORT_DEEPEST_POINT
 
 	btVector3 point;
-	
 
 	// only keep points that are behind the witness face
 	{
-		btVector3 localPlaneNormal (polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
+		btVector3 localPlaneNormal(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
 		btScalar localPlaneEq = polyA.m_plane[3];
-		btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
-		btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
-		for (int i=0;i<pVtxIn->size();i++)
+		btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal;
+		btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin());
+		for (int i = 0; i < pVtxIn->size(); i++)
 		{
 			btVector3 vtx = pVtxIn->at(i);
-			btScalar depth = planeNormalWS.dot(vtx)+planeEqWS;
-			if (depth <=minDist)
+			btScalar depth = planeNormalWS.dot(vtx) + planeEqWS;
+			if (depth <= minDist)
 			{
-//				printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
+				//				printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
 				depth = minDist;
 			}
 
-			if (depth <=maxDist)
+			if (depth <= maxDist)
 			{
 				btVector3 point = pVtxIn->at(i);
 #ifdef ONLY_REPORT_DEEPEST_POINT
@@ -507,40 +495,32 @@ void	btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
 				{
 					printf("error in btPolyhedralContactClipping depth = %f\n", depth);
 					printf("likely wrong separatingNormal passed in\n");
-				} 
-#endif				
-				resultOut.addContactPoint(separatingNormal,point,depth);
+				}
+#endif
+				resultOut.addContactPoint(separatingNormal, point, depth);
 #endif
 			}
 		}
 	}
 #ifdef ONLY_REPORT_DEEPEST_POINT
-	if (curMaxDist<maxDist)
+	if (curMaxDist < maxDist)
 	{
-		resultOut.addContactPoint(separatingNormal,point,curMaxDist);
+		resultOut.addContactPoint(separatingNormal, point, curMaxDist);
 	}
-#endif //ONLY_REPORT_DEEPEST_POINT
-
+#endif  //ONLY_REPORT_DEEPEST_POINT
 }
 
-
-
-
-
-void	btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btVertexArray& worldVertsB1,btVertexArray& worldVertsB2,btDiscreteCollisionDetectorInterface::Result& resultOut)
+void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, const btScalar minDist, btScalar maxDist, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, btDiscreteCollisionDetectorInterface::Result& resultOut)
 {
-
 	btVector3 separatingNormal = separatingNormal1.normalized();
-//	const btVector3 c0 = transA * hullA.m_localCenter;
-//	const btVector3 c1 = transB * hullB.m_localCenter;
+	//	const btVector3 c0 = transA * hullA.m_localCenter;
+	//	const btVector3 c1 = transB * hullB.m_localCenter;
 	//const btVector3 DeltaC2 = c0 - c1;
 
-
-
-	int closestFaceB=-1;
+	int closestFaceB = -1;
 	btScalar dmax = -FLT_MAX;
 	{
-		for(int face=0;face<hullB.m_faces.size();face++)
+		for (int face = 0; face < hullB.m_faces.size(); face++)
 		{
 			const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
 			const btVector3 WorldNormal = transB.getBasis() * Normal;
@@ -553,18 +533,16 @@ void	btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin
 		}
 	}
 	worldVertsB1.resize(0);
-				{
-					const btFace& polyB = hullB.m_faces[closestFaceB];
-					const int numVertices = polyB.m_indices.size();
-					for(int e0=0;e0<numVertices;e0++)
-					{
-						const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
-						worldVertsB1.push_back(transB*b);
-					}
-				}
-
-	
-	if (closestFaceB>=0)
-		clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, worldVertsB2,minDist, maxDist,resultOut);
+	{
+		const btFace& polyB = hullB.m_faces[closestFaceB];
+		const int numVertices = polyB.m_indices.size();
+		for (int e0 = 0; e0 < numVertices; e0++)
+		{
+			const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
+			worldVertsB1.push_back(transB * b);
+		}
+	}
 
+	if (closestFaceB >= 0)
+		clipFaceAgainstHull(separatingNormal, hullA, transA, worldVertsB1, worldVertsB2, minDist, maxDist, resultOut);
 }