Update Bullet to the latest commit 126b676

1 files changed, 140 insertions, 172 deletions

diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp
index 0f9ced554b..f6987cc760 100644
--- a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp
@@ -19,42 +19,37 @@ subject to the following restrictions:
 #include "LinearMath/btQuaternion.h"
 #include "BulletCollision/CollisionShapes/btStridingMeshInterface.h"
 
-
-btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb)
-: btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface)
+btConvexTriangleMeshShape ::btConvexTriangleMeshShape(btStridingMeshInterface* meshInterface, bool calcAabb)
+	: btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface)
 {
 	m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE;
-	if ( calcAabb )
+	if (calcAabb)
 		recalcLocalAabb();
 }
 
-
-
-
 ///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once
 ///but then we are duplicating
-class LocalSupportVertexCallback: public btInternalTriangleIndexCallback
+class LocalSupportVertexCallback : public btInternalTriangleIndexCallback
 {
-
 	btVector3 m_supportVertexLocal;
-public:
 
+public:
 	btScalar m_maxDot;
 	btVector3 m_supportVecLocal;
 
 	LocalSupportVertexCallback(const btVector3& supportVecLocal)
-		: m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)),
-		m_maxDot(btScalar(-BT_LARGE_FLOAT)),
-                m_supportVecLocal(supportVecLocal)
+		: m_supportVertexLocal(btScalar(0.), btScalar(0.), btScalar(0.)),
+		  m_maxDot(btScalar(-BT_LARGE_FLOAT)),
+		  m_supportVecLocal(supportVecLocal)
 	{
 	}
 
-	virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
+	virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
 	{
 		(void)triangleIndex;
 		(void)partId;
 
-		for (int i=0;i<3;i++)
+		for (int i = 0; i < 3; i++)
 		{
 			btScalar dot = m_supportVecLocal.dot(triangle[i]);
 			if (dot > m_maxDot)
@@ -64,99 +59,82 @@ public:
 			}
 		}
 	}
-	
-	btVector3	GetSupportVertexLocal()
+
+	btVector3 GetSupportVertexLocal()
 	{
 		return m_supportVertexLocal;
 	}
-
 };
 
-
-
-
-
-btVector3	btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
+btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const
 {
-	btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
+	btVector3 supVec(btScalar(0.), btScalar(0.), btScalar(0.));
 
 	btVector3 vec = vec0;
 	btScalar lenSqr = vec.length2();
 	if (lenSqr < btScalar(0.0001))
 	{
-		vec.setValue(1,0,0);
-	} else
+		vec.setValue(1, 0, 0);
+	}
+	else
 	{
-		btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
+		btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
 		vec *= rlen;
 	}
 
-	LocalSupportVertexCallback	supportCallback(vec);
-	btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-	m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
+	LocalSupportVertexCallback supportCallback(vec);
+	btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+	m_stridingMesh->InternalProcessAllTriangles(&supportCallback, -aabbMax, aabbMax);
 	supVec = supportCallback.GetSupportVertexLocal();
 
 	return supVec;
 }
 
-void	btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
+void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 {
 	//use 'w' component of supportVerticesOut?
 	{
-		for (int i=0;i<numVectors;i++)
+		for (int i = 0; i < numVectors; i++)
 		{
 			supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT);
 		}
 	}
-	
-	///@todo: could do the batch inside the callback!
 
+	///@todo: could do the batch inside the callback!
 
-	for (int j=0;j<numVectors;j++)
+	for (int j = 0; j < numVectors; j++)
 	{
 		const btVector3& vec = vectors[j];
-		LocalSupportVertexCallback	supportCallback(vec);
-		btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-		m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
+		LocalSupportVertexCallback supportCallback(vec);
+		btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+		m_stridingMesh->InternalProcessAllTriangles(&supportCallback, -aabbMax, aabbMax);
 		supportVerticesOut[j] = supportCallback.GetSupportVertexLocal();
 	}
-	
 }
-	
 
-
-btVector3	btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec)const
+btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) const
 {
 	btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
 
-	if ( getMargin()!=btScalar(0.) )
+	if (getMargin() != btScalar(0.))
 	{
 		btVector3 vecnorm = vec;
-		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
+		if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON))
 		{
-			vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
-		} 
+			vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.));
+		}
 		vecnorm.normalize();
-		supVertex+= getMargin() * vecnorm;
+		supVertex += getMargin() * vecnorm;
 	}
 	return supVertex;
 }
 
-
-
-
-
-
-
-
-
 //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection
 //Please note that you can debug-draw btConvexTriangleMeshShape with the Raytracer Demo
-int	btConvexTriangleMeshShape::getNumVertices() const
+int btConvexTriangleMeshShape::getNumVertices() const
 {
 	//cache this?
 	return 0;
-	
 }
 
 int btConvexTriangleMeshShape::getNumEdges() const
@@ -164,44 +142,40 @@ int btConvexTriangleMeshShape::getNumEdges() const
 	return 0;
 }
 
-void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const
+void btConvexTriangleMeshShape::getEdge(int, btVector3&, btVector3&) const
 {
-	btAssert(0);	
+	btAssert(0);
 }
 
-void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const
+void btConvexTriangleMeshShape::getVertex(int, btVector3&) const
 {
 	btAssert(0);
 }
 
-int	btConvexTriangleMeshShape::getNumPlanes() const
+int btConvexTriangleMeshShape::getNumPlanes() const
 {
 	return 0;
 }
 
-void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int  ) const
+void btConvexTriangleMeshShape::getPlane(btVector3&, btVector3&, int) const
 {
 	btAssert(0);
 }
 
 //not yet
-bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const
+bool btConvexTriangleMeshShape::isInside(const btVector3&, btScalar) const
 {
 	btAssert(0);
 	return false;
 }
 
-
-
-void	btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling)
+void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling)
 {
 	m_stridingMesh->setScaling(scaling);
-	
+
 	recalcLocalAabb();
-	
 }
 
-
 const btVector3& btConvexTriangleMeshShape::getLocalScaling() const
 {
 	return m_stridingMesh->getScaling();
@@ -209,107 +183,101 @@ const btVector3& btConvexTriangleMeshShape::getLocalScaling() const
 
 void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const
 {
-   class CenterCallback: public btInternalTriangleIndexCallback
-   {
-      bool first;
-      btVector3 ref;
-      btVector3 sum;
-      btScalar volume;
-
-   public:
-
-      CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0)
-      {
-      }
-
-      virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
-      {
-         (void) triangleIndex;
-         (void) partId;
-         if (first)
-         {
-            ref = triangle[0];
-            first = false;
-         }
-         else
-         {
-            btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref));
-            sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref));
-            volume += vol;
-         }
-      }
-      
-      btVector3 getCenter()
-      {
-         return (volume > 0) ? sum / volume : ref;
-      }
-
-      btScalar getVolume()
-      {
-         return volume * btScalar(1. / 6);
-      }
-
-   };
-
-   class InertiaCallback: public btInternalTriangleIndexCallback
-   {
-      btMatrix3x3 sum;
-      btVector3 center;
-
-   public:
-
-      InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center)
-      {
-      }
-
-      virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
-      {
-         (void) triangleIndex;
-         (void) partId;
-         btMatrix3x3 i;
-         btVector3 a = triangle[0] - center;
-         btVector3 b = triangle[1] - center;
-         btVector3 c = triangle[2] - center;
-         btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
-         for (int j = 0; j < 3; j++)
-         {
-            for (int k = 0; k <= j; k++)
-            {
-               i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k])
-                  + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j]));
-            }
-         }
-         btScalar i00 = -i[0][0];
-         btScalar i11 = -i[1][1];
-         btScalar i22 = -i[2][2];
-         i[0][0] = i11 + i22; 
-         i[1][1] = i22 + i00; 
-         i[2][2] = i00 + i11;
-         sum[0] += i[0];
-         sum[1] += i[1];
-         sum[2] += i[2];
-      }
-      
-      btMatrix3x3& getInertia()
-      {
-         return sum;
-      }
-
-   };
-
-   CenterCallback centerCallback;
-   btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
-   m_stridingMesh->InternalProcessAllTriangles(&centerCallback, -aabbMax, aabbMax);
-   btVector3 center = centerCallback.getCenter();
-   principal.setOrigin(center);
-   volume = centerCallback.getVolume();
-
-   InertiaCallback inertiaCallback(center);
-   m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax);
-
-   btMatrix3x3& i = inertiaCallback.getInertia();
-   i.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
-   inertia.setValue(i[0][0], i[1][1], i[2][2]);
-   inertia /= volume;
-}
+	class CenterCallback : public btInternalTriangleIndexCallback
+	{
+		bool first;
+		btVector3 ref;
+		btVector3 sum;
+		btScalar volume;
+
+	public:
+		CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0)
+		{
+		}
+
+		virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
+		{
+			(void)triangleIndex;
+			(void)partId;
+			if (first)
+			{
+				ref = triangle[0];
+				first = false;
+			}
+			else
+			{
+				btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref));
+				sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref));
+				volume += vol;
+			}
+		}
 
+		btVector3 getCenter()
+		{
+			return (volume > 0) ? sum / volume : ref;
+		}
+
+		btScalar getVolume()
+		{
+			return volume * btScalar(1. / 6);
+		}
+	};
+
+	class InertiaCallback : public btInternalTriangleIndexCallback
+	{
+		btMatrix3x3 sum;
+		btVector3 center;
+
+	public:
+		InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center)
+		{
+		}
+
+		virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
+		{
+			(void)triangleIndex;
+			(void)partId;
+			btMatrix3x3 i;
+			btVector3 a = triangle[0] - center;
+			btVector3 b = triangle[1] - center;
+			btVector3 c = triangle[2] - center;
+			btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
+			for (int j = 0; j < 3; j++)
+			{
+				for (int k = 0; k <= j; k++)
+				{
+					i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j]));
+				}
+			}
+			btScalar i00 = -i[0][0];
+			btScalar i11 = -i[1][1];
+			btScalar i22 = -i[2][2];
+			i[0][0] = i11 + i22;
+			i[1][1] = i22 + i00;
+			i[2][2] = i00 + i11;
+			sum[0] += i[0];
+			sum[1] += i[1];
+			sum[2] += i[2];
+		}
+
+		btMatrix3x3& getInertia()
+		{
+			return sum;
+		}
+	};
+
+	CenterCallback centerCallback;
+	btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
+	m_stridingMesh->InternalProcessAllTriangles(&centerCallback, -aabbMax, aabbMax);
+	btVector3 center = centerCallback.getCenter();
+	principal.setOrigin(center);
+	volume = centerCallback.getVolume();
+
+	InertiaCallback inertiaCallback(center);
+	m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax);
+
+	btMatrix3x3& i = inertiaCallback.getInertia();
+	i.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
+	inertia.setValue(i[0][0], i[1][1], i[2][2]);
+	inertia /= volume;
+}