Update Bullet to the latest commit 126b676

1 files changed, 73 insertions, 110 deletions

diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
index d8cbe96142..e50f85e2bb 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btConvexConcaveCollisionAlgorithm.h"
 #include "LinearMath/btQuickprof.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
@@ -29,10 +28,10 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
 #include "BulletCollision/CollisionShapes/btSdfCollisionShape.h"
 
-btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
-: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
-m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped),
-m_isSwapped(isSwapped)
+btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped)
+	: btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
+	  m_btConvexTriangleCallback(ci.m_dispatcher1, body0Wrap, body1Wrap, isSwapped),
+	  m_isSwapped(isSwapped)
 {
 }
 
@@ -40,7 +39,7 @@ btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
 {
 }
 
-void	btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray&	manifoldArray)
+void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray)
 {
 	if (m_btConvexTriangleCallback.m_manifoldPtr)
 	{
@@ -48,38 +47,32 @@ void	btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray&
 	}
 }
 
-
-btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher*  dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped):
-	  m_dispatcher(dispatcher),
-	m_dispatchInfoPtr(0)
+btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) : m_dispatcher(dispatcher),
+																																													 m_dispatchInfoPtr(0)
 {
-	m_convexBodyWrap = isSwapped? body1Wrap:body0Wrap;
-	m_triBodyWrap = isSwapped? body0Wrap:body1Wrap;
-	
-	  //
-	  // create the manifold from the dispatcher 'manifold pool'
-	  //
-	  m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(),m_triBodyWrap->getCollisionObject());
+	m_convexBodyWrap = isSwapped ? body1Wrap : body0Wrap;
+	m_triBodyWrap = isSwapped ? body0Wrap : body1Wrap;
+
+	//
+	// create the manifold from the dispatcher 'manifold pool'
+	//
+	m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(), m_triBodyWrap->getCollisionObject());
 
-  	  clearCache();
+	clearCache();
 }
 
 btConvexTriangleCallback::~btConvexTriangleCallback()
 {
 	clearCache();
-	m_dispatcher->releaseManifold( m_manifoldPtr );
-  
+	m_dispatcher->releaseManifold(m_manifoldPtr);
 }
-  
 
-void	btConvexTriangleCallback::clearCache()
+void btConvexTriangleCallback::clearCache()
 {
 	m_dispatcher->clearManifold(m_manifoldPtr);
 }
 
-
-void btConvexTriangleCallback::processTriangle(btVector3* triangle,int
-partId, int triangleIndex)
+void btConvexTriangleCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex)
 {
 	BT_PROFILE("btConvexTriangleCallback::processTriangle");
 
@@ -88,16 +81,12 @@ partId, int triangleIndex)
 		return;
 	}
 
-        //just for debugging purposes
-        //printf("triangle %d",m_triangleCount++);
-
-
+	//just for debugging purposes
+	//printf("triangle %d",m_triangleCount++);
 
 	btCollisionAlgorithmConstructionInfo ci;
 	ci.m_dispatcher1 = m_dispatcher;
 
-
-
 #if 0	
 	
 	///debug drawing of the overlapping triangles
@@ -111,16 +100,15 @@ partId, int triangleIndex)
 		m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
 	}
 #endif
-	
+
 	if (m_convexBodyWrap->getCollisionShape()->isConvex())
 	{
-		btTriangleShape tm(triangle[0],triangle[1],triangle[2]);	
+		btTriangleShape tm(triangle[0], triangle[1], triangle[2]);
 		tm.setMargin(m_collisionMarginTriangle);
-		
-		
-		btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform?
+
+		btCollisionObjectWrapper triObWrap(m_triBodyWrap, &tm, m_triBodyWrap->getCollisionObject(), m_triBodyWrap->getWorldTransform(), partId, triangleIndex);  //correct transform?
 		btCollisionAlgorithm* colAlgo = 0;
-		
+
 		if (m_resultOut->m_closestPointDistanceThreshold > 0)
 		{
 			colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
@@ -135,36 +123,32 @@ partId, int triangleIndex)
 		{
 			tmpWrap = m_resultOut->getBody0Wrap();
 			m_resultOut->setBody0Wrap(&triObWrap);
-			m_resultOut->setShapeIdentifiersA(partId,triangleIndex);
+			m_resultOut->setShapeIdentifiersA(partId, triangleIndex);
 		}
 		else
 		{
 			tmpWrap = m_resultOut->getBody1Wrap();
 			m_resultOut->setBody1Wrap(&triObWrap);
-			m_resultOut->setShapeIdentifiersB(partId,triangleIndex);
+			m_resultOut->setShapeIdentifiersB(partId, triangleIndex);
 		}
-	
-		colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut);
+
+		colAlgo->processCollision(m_convexBodyWrap, &triObWrap, *m_dispatchInfoPtr, m_resultOut);
 
 		if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
 		{
 			m_resultOut->setBody0Wrap(tmpWrap);
-		} else
+		}
+		else
 		{
 			m_resultOut->setBody1Wrap(tmpWrap);
 		}
 
-
-
 		colAlgo->~btCollisionAlgorithm();
 		ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
 	}
-
 }
 
-
-
-void	btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
+void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
 {
 	m_convexBodyWrap = convexBodyWrap;
 	m_triBodyWrap = triBodyWrap;
@@ -185,16 +169,14 @@ void	btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr
 
 	m_aabbMax += extra;
 	m_aabbMin -= extra;
-
 }
 
 void btConvexConcaveCollisionAlgorithm::clearCache()
 {
 	m_btConvexTriangleCallback.clearCache();
-
 }
 
-void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
+void btConvexConcaveCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision");
 
@@ -208,7 +190,6 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec
 			btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape();
 			if (convexBodyWrap->getCollisionShape()->isConvex())
 			{
-
 				btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape();
 				btAlignedObjectArray<btVector3> queryVertices;
 
@@ -229,7 +210,6 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec
 					queryVertices.push_back(btVector3(0, 0, 0));
 					btSphereShape* sphere = (btSphereShape*)convex;
 					maxDist = sphere->getRadius() + SIMD_EPSILON;
-
 				}
 				if (queryVertices.size())
 				{
@@ -240,7 +220,7 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec
 					for (int v = 0; v < queryVertices.size(); v++)
 					{
 						const btVector3& vtx = queryVertices[v];
-						btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform()*vtx;
+						btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform() * vtx;
 						btVector3 vtxInSdf = triBodyWrap->getWorldTransform().invXform(vtxWorldSpace);
 
 						btVector3 normalLocal;
@@ -250,58 +230,52 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec
 							if (dist <= maxDist)
 							{
 								normalLocal.safeNormalize();
-								btVector3 normal = triBodyWrap->getWorldTransform().getBasis()*normalLocal;
+								btVector3 normal = triBodyWrap->getWorldTransform().getBasis() * normalLocal;
 
 								if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
 								{
 									btSphereShape* sphere = (btSphereShape*)convex;
 									dist -= sphere->getRadius();
-									vtxWorldSpace -= sphere->getRadius()*normal;
-
+									vtxWorldSpace -= sphere->getRadius() * normal;
 								}
-								resultOut->addContactPoint(normal,vtxWorldSpace-normal*dist, dist);
+								resultOut->addContactPoint(normal, vtxWorldSpace - normal * dist, dist);
 							}
 						}
 					}
 					resultOut->refreshContactPoints();
 				}
-
 			}
-		} else
+		}
+		else
 		{
-			const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape());
-		
+			const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>(triBodyWrap->getCollisionShape());
+
 			if (convexBodyWrap->getCollisionShape()->isConvex())
 			{
 				btScalar collisionMarginTriangle = concaveShape->getMargin();
-					
+
 				resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
-				m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
+				m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, convexBodyWrap, triBodyWrap, resultOut);
 
-				m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
+				m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(), triBodyWrap->getCollisionObject());
+
+				concaveShape->processAllTriangles(&m_btConvexTriangleCallback, m_btConvexTriangleCallback.getAabbMin(), m_btConvexTriangleCallback.getAabbMax());
 
-				concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
-			
 				resultOut->refreshContactPoints();
 
 				m_btConvexTriangleCallback.clearWrapperData();
-	
 			}
 		}
-	
 	}
-
 }
 
-
-btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	(void)resultOut;
 	(void)dispatchInfo;
 	btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
 	btCollisionObject* triBody = m_isSwapped ? body0 : body1;
 
-
 	//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
 
 	//only perform CCD above a certain threshold, this prevents blocking on the long run
@@ -320,25 +294,23 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj
 	btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
 	btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
 
-	struct LocalTriangleSphereCastCallback	: public btTriangleCallback
+	struct LocalTriangleSphereCastCallback : public btTriangleCallback
 	{
 		btTransform m_ccdSphereFromTrans;
 		btTransform m_ccdSphereToTrans;
-		btTransform	m_meshTransform;
+		btTransform m_meshTransform;
 
-		btScalar	m_ccdSphereRadius;
-		btScalar	m_hitFraction;
-	
+		btScalar m_ccdSphereRadius;
+		btScalar m_hitFraction;
 
-		LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
-			:m_ccdSphereFromTrans(from),
-			m_ccdSphereToTrans(to),
-			m_ccdSphereRadius(ccdSphereRadius),
-			m_hitFraction(hitFraction)
-		{			
+		LocalTriangleSphereCastCallback(const btTransform& from, const btTransform& to, btScalar ccdSphereRadius, btScalar hitFraction)
+			: m_ccdSphereFromTrans(from),
+			  m_ccdSphereToTrans(to),
+			  m_ccdSphereRadius(ccdSphereRadius),
+			  m_hitFraction(hitFraction)
+		{
 		}
-		
-		
+
 		virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 		{
 			BT_PROFILE("processTriangle");
@@ -349,29 +321,23 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj
 			ident.setIdentity();
 			btConvexCast::CastResult castResult;
 			castResult.m_fraction = m_hitFraction;
-			btSphereShape	pointShape(m_ccdSphereRadius);
-			btTriangleShape	triShape(triangle[0],triangle[1],triangle[2]);
-			btVoronoiSimplexSolver	simplexSolver;
-			btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
+			btSphereShape pointShape(m_ccdSphereRadius);
+			btTriangleShape triShape(triangle[0], triangle[1], triangle[2]);
+			btVoronoiSimplexSolver simplexSolver;
+			btSubsimplexConvexCast convexCaster(&pointShape, &triShape, &simplexSolver);
 			//GjkConvexCast	convexCaster(&pointShape,convexShape,&simplexSolver);
 			//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
 			//local space?
 
-			if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
-				ident,ident,castResult))
+			if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans,
+											  ident, ident, castResult))
 			{
 				if (m_hitFraction > castResult.m_fraction)
 					m_hitFraction = castResult.m_fraction;
 			}
-
 		}
-
 	};
 
-
-	
-
-	
 	if (triBody->getCollisionShape()->isConcave())
 	{
 		btVector3 rayAabbMin = convexFromLocal.getOrigin();
@@ -379,33 +345,30 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj
 		btVector3 rayAabbMax = convexFromLocal.getOrigin();
 		rayAabbMax.setMax(convexToLocal.getOrigin());
 		btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
-		rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
-		rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
+		rayAabbMin -= btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
+		rayAabbMax += btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
 
-		btScalar curHitFraction = btScalar(1.); //is this available?
-		LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
-			convexbody->getCcdSweptSphereRadius(),curHitFraction);
+		btScalar curHitFraction = btScalar(1.);  //is this available?
+		LocalTriangleSphereCastCallback raycastCallback(convexFromLocal, convexToLocal,
+														convexbody->getCcdSweptSphereRadius(), curHitFraction);
 
 		raycastCallback.m_hitFraction = convexbody->getHitFraction();
 
 		btCollisionObject* concavebody = triBody;
 
-		btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
-		
+		btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape();
+
 		if (triangleMesh)
 		{
-			triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
+			triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax);
 		}
-	
-
 
 		if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
 		{
-			convexbody->setHitFraction( raycastCallback.m_hitFraction);
+			convexbody->setHitFraction(raycastCallback.m_hitFraction);
 			return raycastCallback.m_hitFraction;
 		}
 	}
 
 	return btScalar(1.);
-
 }