Update Bullet to the latest commit 126b676

1 files changed, 229 insertions, 264 deletions

diff --git a/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp b/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp
index cf3d5ef49d..dae61d4581 100644
--- a/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp
+++ b/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp
@@ -23,26 +23,24 @@ subject to the following restrictions:
 		
 */
 
-
 #include "b3VoronoiSimplexSolver.h"
 
-#define VERTA  0
-#define VERTB  1
-#define VERTC  2
-#define VERTD  3
+#define VERTA 0
+#define VERTB 1
+#define VERTC 2
+#define VERTD 3
 
 #define B3_CATCH_DEGENERATE_TETRAHEDRON 1
-void	b3VoronoiSimplexSolver::removeVertex(int index)
+void b3VoronoiSimplexSolver::removeVertex(int index)
 {
-	
-	b3Assert(m_numVertices>0);
+	b3Assert(m_numVertices > 0);
 	m_numVertices--;
 	m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
 	m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
 	m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
 }
 
-void	b3VoronoiSimplexSolver::reduceVertices (const b3UsageBitfield& usedVerts)
+void b3VoronoiSimplexSolver::reduceVertices(const b3UsageBitfield& usedVerts)
 {
 	if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
 		removeVertex(3);
@@ -52,29 +50,22 @@ void	b3VoronoiSimplexSolver::reduceVertices (const b3UsageBitfield& usedVerts)
 
 	if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
 		removeVertex(1);
-	
+
 	if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
 		removeVertex(0);
-
 }
 
-
-
-
-
 //clear the simplex, remove all the vertices
 void b3VoronoiSimplexSolver::reset()
 {
 	m_cachedValidClosest = false;
 	m_numVertices = 0;
 	m_needsUpdate = true;
-	m_lastW = b3MakeVector3(b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT));
+	m_lastW = b3MakeVector3(b3Scalar(B3_LARGE_FLOAT), b3Scalar(B3_LARGE_FLOAT), b3Scalar(B3_LARGE_FLOAT));
 	m_cachedBC.reset();
 }
 
-
-
-	//add a vertex
+//add a vertex
 void b3VoronoiSimplexSolver::addVertex(const b3Vector3& w, const b3Vector3& p, const b3Vector3& q)
 {
 	m_lastW = w;
@@ -87,9 +78,8 @@ void b3VoronoiSimplexSolver::addVertex(const b3Vector3& w, const b3Vector3& p, c
 	m_numVertices++;
 }
 
-bool	b3VoronoiSimplexSolver::updateClosestVectorAndPoints()
+bool b3VoronoiSimplexSolver::updateClosestVectorAndPoints()
 {
-	
 	if (m_needsUpdate)
 	{
 		m_cachedBC.reset();
@@ -98,127 +88,131 @@ bool	b3VoronoiSimplexSolver::updateClosestVectorAndPoints()
 
 		switch (numVertices())
 		{
-		case 0:
+			case 0:
 				m_cachedValidClosest = false;
 				break;
-		case 1:
+			case 1:
 			{
 				m_cachedP1 = m_simplexPointsP[0];
 				m_cachedP2 = m_simplexPointsQ[0];
-				m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
+				m_cachedV = m_cachedP1 - m_cachedP2;  //== m_simplexVectorW[0]
 				m_cachedBC.reset();
-				m_cachedBC.setBarycentricCoordinates(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
+				m_cachedBC.setBarycentricCoordinates(b3Scalar(1.), b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
 				m_cachedValidClosest = m_cachedBC.isValid();
 				break;
 			};
-		case 2:
+			case 2:
 			{
-			//closest point origin from line segment
-					const b3Vector3& from = m_simplexVectorW[0];
-					const b3Vector3& to = m_simplexVectorW[1];
-					b3Vector3 nearest;
-
-					b3Vector3 p =b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
-					b3Vector3 diff = p - from;
-					b3Vector3 v = to - from;
-					b3Scalar t = v.dot(diff);
-					
-					if (t > 0) {
-						b3Scalar dotVV = v.dot(v);
-						if (t < dotVV) {
-							t /= dotVV;
-							diff -= t*v;
-							m_cachedBC.m_usedVertices.usedVertexA = true;
-							m_cachedBC.m_usedVertices.usedVertexB = true;
-						} else {
-							t = 1;
-							diff -= v;
-							//reduce to 1 point
-							m_cachedBC.m_usedVertices.usedVertexB = true;
-						}
-					} else
+				//closest point origin from line segment
+				const b3Vector3& from = m_simplexVectorW[0];
+				const b3Vector3& to = m_simplexVectorW[1];
+				b3Vector3 nearest;
+
+				b3Vector3 p = b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
+				b3Vector3 diff = p - from;
+				b3Vector3 v = to - from;
+				b3Scalar t = v.dot(diff);
+
+				if (t > 0)
+				{
+					b3Scalar dotVV = v.dot(v);
+					if (t < dotVV)
 					{
-						t = 0;
-						//reduce to 1 point
+						t /= dotVV;
+						diff -= t * v;
 						m_cachedBC.m_usedVertices.usedVertexA = true;
+						m_cachedBC.m_usedVertices.usedVertexB = true;
+					}
+					else
+					{
+						t = 1;
+						diff -= v;
+						//reduce to 1 point
+						m_cachedBC.m_usedVertices.usedVertexB = true;
 					}
-					m_cachedBC.setBarycentricCoordinates(1-t,t);
-					nearest = from + t*v;
+				}
+				else
+				{
+					t = 0;
+					//reduce to 1 point
+					m_cachedBC.m_usedVertices.usedVertexA = true;
+				}
+				m_cachedBC.setBarycentricCoordinates(1 - t, t);
+				nearest = from + t * v;
 
-					m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
-					m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
-					m_cachedV = m_cachedP1 - m_cachedP2;
-					
-					reduceVertices(m_cachedBC.m_usedVertices);
+				m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
+				m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
+				m_cachedV = m_cachedP1 - m_cachedP2;
 
-					m_cachedValidClosest = m_cachedBC.isValid();
-					break;
+				reduceVertices(m_cachedBC.m_usedVertices);
+
+				m_cachedValidClosest = m_cachedBC.isValid();
+				break;
 			}
-		case 3: 
-			{ 
-				//closest point origin from triangle 
-				b3Vector3 p =b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); 
+			case 3:
+			{
+				//closest point origin from triangle
+				b3Vector3 p = b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
 
-				const b3Vector3& a = m_simplexVectorW[0]; 
-				const b3Vector3& b = m_simplexVectorW[1]; 
-				const b3Vector3& c = m_simplexVectorW[2]; 
+				const b3Vector3& a = m_simplexVectorW[0];
+				const b3Vector3& b = m_simplexVectorW[1];
+				const b3Vector3& c = m_simplexVectorW[2];
 
-				closestPtPointTriangle(p,a,b,c,m_cachedBC); 
-				m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + 
-				m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + 
-				m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2]; 
+				closestPtPointTriangle(p, a, b, c, m_cachedBC);
+				m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
+							 m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
+							 m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2];
 
-				m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + 
-				m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + 
-				m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2]; 
+				m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
+							 m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
+							 m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2];
 
-				m_cachedV = m_cachedP1-m_cachedP2; 
+				m_cachedV = m_cachedP1 - m_cachedP2;
 
-				reduceVertices (m_cachedBC.m_usedVertices); 
-				m_cachedValidClosest = m_cachedBC.isValid(); 
+				reduceVertices(m_cachedBC.m_usedVertices);
+				m_cachedValidClosest = m_cachedBC.isValid();
 
-				break; 
+				break;
 			}
-		case 4:
+			case 4:
 			{
+				b3Vector3 p = b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
 
-				
-				b3Vector3 p =b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
-				
 				const b3Vector3& a = m_simplexVectorW[0];
 				const b3Vector3& b = m_simplexVectorW[1];
 				const b3Vector3& c = m_simplexVectorW[2];
 				const b3Vector3& d = m_simplexVectorW[3];
 
-				bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
+				bool hasSeperation = closestPtPointTetrahedron(p, a, b, c, d, m_cachedBC);
 
 				if (hasSeperation)
 				{
-
 					m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
-						m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
-						m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
-						m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
+								 m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
+								 m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
+								 m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
 
 					m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
-						m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
-						m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
-						m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
+								 m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
+								 m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
+								 m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
 
-					m_cachedV = m_cachedP1-m_cachedP2;
-					reduceVertices (m_cachedBC.m_usedVertices);
-				} else
+					m_cachedV = m_cachedP1 - m_cachedP2;
+					reduceVertices(m_cachedBC.m_usedVertices);
+				}
+				else
 				{
-//					printf("sub distance got penetration\n");
+					//					printf("sub distance got penetration\n");
 
 					if (m_cachedBC.m_degenerate)
 					{
 						m_cachedValidClosest = false;
-					} else
+					}
+					else
 					{
 						m_cachedValidClosest = true;
 						//degenerate case == false, penetration = true + zero
-						m_cachedV.setValue(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
+						m_cachedV.setValue(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
 					}
 					break;
 				}
@@ -228,7 +222,7 @@ bool	b3VoronoiSimplexSolver::updateClosestVectorAndPoints()
 				//closest point origin from tetrahedron
 				break;
 			}
-		default:
+			default:
 			{
 				m_cachedValidClosest = false;
 			}
@@ -236,7 +230,6 @@ bool	b3VoronoiSimplexSolver::updateClosestVectorAndPoints()
 	}
 
 	return m_cachedValidClosest;
-
 }
 
 //return/calculate the closest vertex
@@ -247,13 +240,11 @@ bool b3VoronoiSimplexSolver::closest(b3Vector3& v)
 	return succes;
 }
 
-
-
 b3Scalar b3VoronoiSimplexSolver::maxVertex()
 {
 	int i, numverts = numVertices();
 	b3Scalar maxV = b3Scalar(0.);
-	for (i=0;i<numverts;i++)
+	for (i = 0; i < numverts; i++)
 	{
 		b3Scalar curLen2 = m_simplexVectorW[i].length2();
 		if (maxV < curLen2)
@@ -262,13 +253,11 @@ b3Scalar b3VoronoiSimplexSolver::maxVertex()
 	return maxV;
 }
 
-
-
-	//return the current simplex
-int b3VoronoiSimplexSolver::getSimplex(b3Vector3 *pBuf, b3Vector3 *qBuf, b3Vector3 *yBuf) const
+//return the current simplex
+int b3VoronoiSimplexSolver::getSimplex(b3Vector3* pBuf, b3Vector3* qBuf, b3Vector3* yBuf) const
 {
 	int i;
-	for (i=0;i<numVertices();i++)
+	for (i = 0; i < numVertices(); i++)
 	{
 		yBuf[i] = m_simplexVectorW[i];
 		pBuf[i] = m_simplexPointsP[i];
@@ -277,20 +266,17 @@ int b3VoronoiSimplexSolver::getSimplex(b3Vector3 *pBuf, b3Vector3 *qBuf, b3Vecto
 	return numVertices();
 }
 
-
-
-
 bool b3VoronoiSimplexSolver::inSimplex(const b3Vector3& w)
 {
 	bool found = false;
 	int i, numverts = numVertices();
 	//b3Scalar maxV = b3Scalar(0.);
-	
+
 	//w is in the current (reduced) simplex
-	for (i=0;i<numverts;i++)
+	for (i = 0; i < numverts; i++)
 	{
 #ifdef BT_USE_EQUAL_VERTEX_THRESHOLD
-		if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold)
+		if (m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold)
 #else
 		if (m_simplexVectorW[i] == w)
 #endif
@@ -300,199 +286,190 @@ bool b3VoronoiSimplexSolver::inSimplex(const b3Vector3& w)
 	//check in case lastW is already removed
 	if (w == m_lastW)
 		return true;
-    	
+
 	return found;
 }
 
-void b3VoronoiSimplexSolver::backup_closest(b3Vector3& v) 
+void b3VoronoiSimplexSolver::backup_closest(b3Vector3& v)
 {
 	v = m_cachedV;
 }
 
-
-bool b3VoronoiSimplexSolver::emptySimplex() const 
+bool b3VoronoiSimplexSolver::emptySimplex() const
 {
 	return (numVertices() == 0);
-
 }
 
-void b3VoronoiSimplexSolver::compute_points(b3Vector3& p1, b3Vector3& p2) 
+void b3VoronoiSimplexSolver::compute_points(b3Vector3& p1, b3Vector3& p2)
 {
 	updateClosestVectorAndPoints();
 	p1 = m_cachedP1;
 	p2 = m_cachedP2;
-
 }
 
-
-
-
-bool	b3VoronoiSimplexSolver::closestPtPointTriangle(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c,b3SubSimplexClosestResult& result)
+bool b3VoronoiSimplexSolver::closestPtPointTriangle(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, b3SubSimplexClosestResult& result)
 {
 	result.m_usedVertices.reset();
 
-    // Check if P in vertex region outside A
-    b3Vector3 ab = b - a;
-    b3Vector3 ac = c - a;
-    b3Vector3 ap = p - a;
-    b3Scalar d1 = ab.dot(ap);
-    b3Scalar d2 = ac.dot(ap);
-    if (d1 <= b3Scalar(0.0) && d2 <= b3Scalar(0.0)) 
+	// Check if P in vertex region outside A
+	b3Vector3 ab = b - a;
+	b3Vector3 ac = c - a;
+	b3Vector3 ap = p - a;
+	b3Scalar d1 = ab.dot(ap);
+	b3Scalar d2 = ac.dot(ap);
+	if (d1 <= b3Scalar(0.0) && d2 <= b3Scalar(0.0))
 	{
 		result.m_closestPointOnSimplex = a;
 		result.m_usedVertices.usedVertexA = true;
-		result.setBarycentricCoordinates(1,0,0);
-		return true;// a; // barycentric coordinates (1,0,0)
+		result.setBarycentricCoordinates(1, 0, 0);
+		return true;  // a; // barycentric coordinates (1,0,0)
 	}
 
-    // Check if P in vertex region outside B
-    b3Vector3 bp = p - b;
-    b3Scalar d3 = ab.dot(bp);
-    b3Scalar d4 = ac.dot(bp);
-    if (d3 >= b3Scalar(0.0) && d4 <= d3) 
+	// Check if P in vertex region outside B
+	b3Vector3 bp = p - b;
+	b3Scalar d3 = ab.dot(bp);
+	b3Scalar d4 = ac.dot(bp);
+	if (d3 >= b3Scalar(0.0) && d4 <= d3)
 	{
 		result.m_closestPointOnSimplex = b;
 		result.m_usedVertices.usedVertexB = true;
-		result.setBarycentricCoordinates(0,1,0);
+		result.setBarycentricCoordinates(0, 1, 0);
 
-		return true; // b; // barycentric coordinates (0,1,0)
+		return true;  // b; // barycentric coordinates (0,1,0)
 	}
-    // Check if P in edge region of AB, if so return projection of P onto AB
-    b3Scalar vc = d1*d4 - d3*d2;
-    if (vc <= b3Scalar(0.0) && d1 >= b3Scalar(0.0) && d3 <= b3Scalar(0.0)) {
-        b3Scalar v = d1 / (d1 - d3);
+	// Check if P in edge region of AB, if so return projection of P onto AB
+	b3Scalar vc = d1 * d4 - d3 * d2;
+	if (vc <= b3Scalar(0.0) && d1 >= b3Scalar(0.0) && d3 <= b3Scalar(0.0))
+	{
+		b3Scalar v = d1 / (d1 - d3);
 		result.m_closestPointOnSimplex = a + v * ab;
 		result.m_usedVertices.usedVertexA = true;
 		result.m_usedVertices.usedVertexB = true;
-		result.setBarycentricCoordinates(1-v,v,0);
+		result.setBarycentricCoordinates(1 - v, v, 0);
 		return true;
-        //return a + v * ab; // barycentric coordinates (1-v,v,0)
-    }
-
-    // Check if P in vertex region outside C
-    b3Vector3 cp = p - c;
-    b3Scalar d5 = ab.dot(cp);
-    b3Scalar d6 = ac.dot(cp);
-    if (d6 >= b3Scalar(0.0) && d5 <= d6) 
+		//return a + v * ab; // barycentric coordinates (1-v,v,0)
+	}
+
+	// Check if P in vertex region outside C
+	b3Vector3 cp = p - c;
+	b3Scalar d5 = ab.dot(cp);
+	b3Scalar d6 = ac.dot(cp);
+	if (d6 >= b3Scalar(0.0) && d5 <= d6)
 	{
 		result.m_closestPointOnSimplex = c;
 		result.m_usedVertices.usedVertexC = true;
-		result.setBarycentricCoordinates(0,0,1);
-		return true;//c; // barycentric coordinates (0,0,1)
+		result.setBarycentricCoordinates(0, 0, 1);
+		return true;  //c; // barycentric coordinates (0,0,1)
 	}
 
-    // Check if P in edge region of AC, if so return projection of P onto AC
-    b3Scalar vb = d5*d2 - d1*d6;
-    if (vb <= b3Scalar(0.0) && d2 >= b3Scalar(0.0) && d6 <= b3Scalar(0.0)) {
-        b3Scalar w = d2 / (d2 - d6);
+	// Check if P in edge region of AC, if so return projection of P onto AC
+	b3Scalar vb = d5 * d2 - d1 * d6;
+	if (vb <= b3Scalar(0.0) && d2 >= b3Scalar(0.0) && d6 <= b3Scalar(0.0))
+	{
+		b3Scalar w = d2 / (d2 - d6);
 		result.m_closestPointOnSimplex = a + w * ac;
 		result.m_usedVertices.usedVertexA = true;
 		result.m_usedVertices.usedVertexC = true;
-		result.setBarycentricCoordinates(1-w,0,w);
+		result.setBarycentricCoordinates(1 - w, 0, w);
 		return true;
-        //return a + w * ac; // barycentric coordinates (1-w,0,w)
-    }
+		//return a + w * ac; // barycentric coordinates (1-w,0,w)
+	}
+
+	// Check if P in edge region of BC, if so return projection of P onto BC
+	b3Scalar va = d3 * d6 - d5 * d4;
+	if (va <= b3Scalar(0.0) && (d4 - d3) >= b3Scalar(0.0) && (d5 - d6) >= b3Scalar(0.0))
+	{
+		b3Scalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
 
-    // Check if P in edge region of BC, if so return projection of P onto BC
-    b3Scalar va = d3*d6 - d5*d4;
-    if (va <= b3Scalar(0.0) && (d4 - d3) >= b3Scalar(0.0) && (d5 - d6) >= b3Scalar(0.0)) {
-        b3Scalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
-		
 		result.m_closestPointOnSimplex = b + w * (c - b);
 		result.m_usedVertices.usedVertexB = true;
 		result.m_usedVertices.usedVertexC = true;
-		result.setBarycentricCoordinates(0,1-w,w);
-		return true;		
-       // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
-    }
-
-    // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
-    b3Scalar denom = b3Scalar(1.0) / (va + vb + vc);
-    b3Scalar v = vb * denom;
-    b3Scalar w = vc * denom;
-    
+		result.setBarycentricCoordinates(0, 1 - w, w);
+		return true;
+		// return b + w * (c - b); // barycentric coordinates (0,1-w,w)
+	}
+
+	// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
+	b3Scalar denom = b3Scalar(1.0) / (va + vb + vc);
+	b3Scalar v = vb * denom;
+	b3Scalar w = vc * denom;
+
 	result.m_closestPointOnSimplex = a + ab * v + ac * w;
 	result.m_usedVertices.usedVertexA = true;
 	result.m_usedVertices.usedVertexB = true;
 	result.m_usedVertices.usedVertexC = true;
-	result.setBarycentricCoordinates(1-v-w,v,w);
-	
-	return true;
-//	return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = b3Scalar(1.0) - v - w
+	result.setBarycentricCoordinates(1 - v - w, v, w);
 
+	return true;
+	//	return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = b3Scalar(1.0) - v - w
 }
 
-
-
-
-
 /// Test if point p and d lie on opposite sides of plane through abc
 int b3VoronoiSimplexSolver::pointOutsideOfPlane(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d)
 {
-	b3Vector3 normal = (b-a).cross(c-a);
+	b3Vector3 normal = (b - a).cross(c - a);
 
-    b3Scalar signp = (p - a).dot(normal); // [AP AB AC]
-    b3Scalar signd = (d - a).dot( normal); // [AD AB AC]
+	b3Scalar signp = (p - a).dot(normal);  // [AP AB AC]
+	b3Scalar signd = (d - a).dot(normal);  // [AD AB AC]
 
 #ifdef B3_CATCH_DEGENERATE_TETRAHEDRON
 #ifdef BT_USE_DOUBLE_PRECISION
-if (signd * signd < (b3Scalar(1e-8) * b3Scalar(1e-8)))
+	if (signd * signd < (b3Scalar(1e-8) * b3Scalar(1e-8)))
 	{
 		return -1;
 	}
 #else
 	if (signd * signd < (b3Scalar(1e-4) * b3Scalar(1e-4)))
 	{
-//		printf("affine dependent/degenerate\n");//
+		//		printf("affine dependent/degenerate\n");//
 		return -1;
 	}
 #endif
 
 #endif
 	// Points on opposite sides if expression signs are opposite
-    return signp * signd < b3Scalar(0.);
+	return signp * signd < b3Scalar(0.);
 }
 
-
-bool	b3VoronoiSimplexSolver::closestPtPointTetrahedron(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d, b3SubSimplexClosestResult& finalResult)
+bool b3VoronoiSimplexSolver::closestPtPointTetrahedron(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d, b3SubSimplexClosestResult& finalResult)
 {
 	b3SubSimplexClosestResult tempResult;
 
-    // Start out assuming point inside all halfspaces, so closest to itself
+	// Start out assuming point inside all halfspaces, so closest to itself
 	finalResult.m_closestPointOnSimplex = p;
 	finalResult.m_usedVertices.reset();
-    finalResult.m_usedVertices.usedVertexA = true;
+	finalResult.m_usedVertices.usedVertexA = true;
 	finalResult.m_usedVertices.usedVertexB = true;
 	finalResult.m_usedVertices.usedVertexC = true;
 	finalResult.m_usedVertices.usedVertexD = true;
 
-    int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
+	int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
 	int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
-  	int	pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
-	int	pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
-
-   if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
-   {
-	   finalResult.m_degenerate = true;
-	   return false;
-   }
+	int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
+	int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
 
-   if (!pointOutsideABC  && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
-	 {
-		 return false;
-	 }
+	if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
+	{
+		finalResult.m_degenerate = true;
+		return false;
+	}
 
+	if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
+	{
+		return false;
+	}
 
-    b3Scalar bestSqDist = FLT_MAX;
-    // If point outside face abc then compute closest point on abc
-	if (pointOutsideABC) 
+	b3Scalar bestSqDist = FLT_MAX;
+	// If point outside face abc then compute closest point on abc
+	if (pointOutsideABC)
 	{
-        closestPtPointTriangle(p, a, b, c,tempResult);
+		closestPtPointTriangle(p, a, b, c, tempResult);
 		b3Vector3 q = tempResult.m_closestPointOnSimplex;
-		
-        b3Scalar sqDist = (q - p).dot( q - p);
-        // Update best closest point if (squared) distance is less than current best
-        if (sqDist < bestSqDist) {
+
+		b3Scalar sqDist = (q - p).dot(q - p);
+		// Update best closest point if (squared) distance is less than current best
+		if (sqDist < bestSqDist)
+		{
 			bestSqDist = sqDist;
 			finalResult.m_closestPointOnSimplex = q;
 			//convert result bitmask!
@@ -501,25 +478,22 @@ bool	b3VoronoiSimplexSolver::closestPtPointTetrahedron(const b3Vector3& p, const
 			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
 			finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
 			finalResult.setBarycentricCoordinates(
-					tempResult.m_barycentricCoords[VERTA],
-					tempResult.m_barycentricCoords[VERTB],
-					tempResult.m_barycentricCoords[VERTC],
-					0
-			);
-
+				tempResult.m_barycentricCoords[VERTA],
+				tempResult.m_barycentricCoords[VERTB],
+				tempResult.m_barycentricCoords[VERTC],
+				0);
 		}
-    }
-  
+	}
 
 	// Repeat test for face acd
-	if (pointOutsideACD) 
+	if (pointOutsideACD)
 	{
-        closestPtPointTriangle(p, a, c, d,tempResult);
+		closestPtPointTriangle(p, a, c, d, tempResult);
 		b3Vector3 q = tempResult.m_closestPointOnSimplex;
 		//convert result bitmask!
 
-        b3Scalar sqDist = (q - p).dot( q - p);
-        if (sqDist < bestSqDist) 
+		b3Scalar sqDist = (q - p).dot(q - p);
+		if (sqDist < bestSqDist)
 		{
 			bestSqDist = sqDist;
 			finalResult.m_closestPointOnSimplex = q;
@@ -529,52 +503,46 @@ bool	b3VoronoiSimplexSolver::closestPtPointTetrahedron(const b3Vector3& p, const
 			finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
 			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
 			finalResult.setBarycentricCoordinates(
-					tempResult.m_barycentricCoords[VERTA],
-					0,
-					tempResult.m_barycentricCoords[VERTB],
-					tempResult.m_barycentricCoords[VERTC]
-			);
-
+				tempResult.m_barycentricCoords[VERTA],
+				0,
+				tempResult.m_barycentricCoords[VERTB],
+				tempResult.m_barycentricCoords[VERTC]);
 		}
-    }
-    // Repeat test for face adb
+	}
+	// Repeat test for face adb
 
-	
 	if (pointOutsideADB)
 	{
-		closestPtPointTriangle(p, a, d, b,tempResult);
+		closestPtPointTriangle(p, a, d, b, tempResult);
 		b3Vector3 q = tempResult.m_closestPointOnSimplex;
 		//convert result bitmask!
 
-        b3Scalar sqDist = (q - p).dot( q - p);
-        if (sqDist < bestSqDist) 
+		b3Scalar sqDist = (q - p).dot(q - p);
+		if (sqDist < bestSqDist)
 		{
 			bestSqDist = sqDist;
 			finalResult.m_closestPointOnSimplex = q;
 			finalResult.m_usedVertices.reset();
 			finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
 			finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
-			
+
 			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
 			finalResult.setBarycentricCoordinates(
-					tempResult.m_barycentricCoords[VERTA],
-					tempResult.m_barycentricCoords[VERTC],
-					0,
-					tempResult.m_barycentricCoords[VERTB]
-			);
-
+				tempResult.m_barycentricCoords[VERTA],
+				tempResult.m_barycentricCoords[VERTC],
+				0,
+				tempResult.m_barycentricCoords[VERTB]);
 		}
-    }
-    // Repeat test for face bdc
-    
+	}
+	// Repeat test for face bdc
 
 	if (pointOutsideBDC)
 	{
-        closestPtPointTriangle(p, b, d, c,tempResult);
+		closestPtPointTriangle(p, b, d, c, tempResult);
 		b3Vector3 q = tempResult.m_closestPointOnSimplex;
 		//convert result bitmask!
-        b3Scalar sqDist = (q - p).dot( q - p);
-        if (sqDist < bestSqDist) 
+		b3Scalar sqDist = (q - p).dot(q - p);
+		if (sqDist < bestSqDist)
 		{
 			bestSqDist = sqDist;
 			finalResult.m_closestPointOnSimplex = q;
@@ -585,25 +553,22 @@ bool	b3VoronoiSimplexSolver::closestPtPointTetrahedron(const b3Vector3& p, const
 			finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
 
 			finalResult.setBarycentricCoordinates(
-					0,
-					tempResult.m_barycentricCoords[VERTA],
-					tempResult.m_barycentricCoords[VERTC],
-					tempResult.m_barycentricCoords[VERTB]
-			);
-
+				0,
+				tempResult.m_barycentricCoords[VERTA],
+				tempResult.m_barycentricCoords[VERTC],
+				tempResult.m_barycentricCoords[VERTB]);
 		}
-    }
+	}
 
 	//help! we ended up full !
-	
+
 	if (finalResult.m_usedVertices.usedVertexA &&
 		finalResult.m_usedVertices.usedVertexB &&
 		finalResult.m_usedVertices.usedVertexC &&
-		finalResult.m_usedVertices.usedVertexD) 
+		finalResult.m_usedVertices.usedVertexD)
 	{
 		return true;
 	}
 
-    return true;
+	return true;
 }
-