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

Update Bullet physics to commit 126b676

1 files changed, 104 insertions, 127 deletions

diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
index 91b7809c17..633bee4825 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
@@ -25,62 +25,58 @@ subject to the following restrictions:
 
 btShapePairCallback gCompoundChildShapePairCallback = 0;
 
-btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
-:btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
-m_isSwapped(isSwapped),
-m_sharedManifold(ci.m_manifold)
+btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped)
+	: btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
+	  m_isSwapped(isSwapped),
+	  m_sharedManifold(ci.m_manifold)
 {
 	m_ownsManifold = false;
 
-	const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
-	btAssert (colObjWrap->getCollisionShape()->isCompound());
-	
+	const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
+	btAssert(colObjWrap->getCollisionShape()->isCompound());
+
 	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
 	m_compoundShapeRevision = compoundShape->getUpdateRevision();
-	
-	
-	preallocateChildAlgorithms(body0Wrap,body1Wrap);
+
+	preallocateChildAlgorithms(body0Wrap, body1Wrap);
 }
 
-void	btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
+void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
 {
-	const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
-	const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap;
-	btAssert (colObjWrap->getCollisionShape()->isCompound());
-	
+	const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
+	const btCollisionObjectWrapper* otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
+	btAssert(colObjWrap->getCollisionShape()->isCompound());
+
 	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
 
 	int numChildren = compoundShape->getNumChildShapes();
 	int i;
-	
+
 	m_childCollisionAlgorithms.resize(numChildren);
-	for (i=0;i<numChildren;i++)
+	for (i = 0; i < numChildren; i++)
 	{
 		if (compoundShape->getDynamicAabbTree())
 		{
 			m_childCollisionAlgorithms[i] = 0;
-		} else
+		}
+		else
 		{
-			
 			const btCollisionShape* childShape = compoundShape->getChildShape(i);
 
-			btCollisionObjectWrapper childWrap(colObjWrap,childShape,colObjWrap->getCollisionObject(),colObjWrap->getWorldTransform(),-1,i);//wrong child trans, but unused (hopefully)
-			m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
-
+			btCollisionObjectWrapper childWrap(colObjWrap, childShape, colObjWrap->getCollisionObject(), colObjWrap->getWorldTransform(), -1, i);  //wrong child trans, but unused (hopefully)
+			m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap, otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
 
 			btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsContact;
 			btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsClosestPoints;
-
-
 		}
 	}
 }
 
-void	btCompoundCollisionAlgorithm::removeChildAlgorithms()
+void btCompoundCollisionAlgorithm::removeChildAlgorithms()
 {
 	int numChildren = m_childCollisionAlgorithms.size();
 	int i;
-	for (i=0;i<numChildren;i++)
+	for (i = 0; i < numChildren; i++)
 	{
 		if (m_childCollisionAlgorithms[i])
 		{
@@ -95,66 +91,56 @@ btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
 	removeChildAlgorithms();
 }
 
-
-
-
-struct	btCompoundLeafCallback : btDbvt::ICollide
+struct btCompoundLeafCallback : btDbvt::ICollide
 {
-
 public:
-
 	const btCollisionObjectWrapper* m_compoundColObjWrap;
 	const btCollisionObjectWrapper* m_otherObjWrap;
 	btDispatcher* m_dispatcher;
 	const btDispatcherInfo& m_dispatchInfo;
-	btManifoldResult*	m_resultOut;
-	btCollisionAlgorithm**	m_childCollisionAlgorithms;
-	btPersistentManifold*	m_sharedManifold;
-	
-	btCompoundLeafCallback (const btCollisionObjectWrapper* compoundObjWrap,const btCollisionObjectWrapper* otherObjWrap,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult*	resultOut,btCollisionAlgorithm**	childCollisionAlgorithms,btPersistentManifold*	sharedManifold)
-		:m_compoundColObjWrap(compoundObjWrap),m_otherObjWrap(otherObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut),
-		m_childCollisionAlgorithms(childCollisionAlgorithms),
-		m_sharedManifold(sharedManifold)
-	{
+	btManifoldResult* m_resultOut;
+	btCollisionAlgorithm** m_childCollisionAlgorithms;
+	btPersistentManifold* m_sharedManifold;
 
+	btCompoundLeafCallback(const btCollisionObjectWrapper* compoundObjWrap, const btCollisionObjectWrapper* otherObjWrap, btDispatcher* dispatcher, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut, btCollisionAlgorithm** childCollisionAlgorithms, btPersistentManifold* sharedManifold)
+		: m_compoundColObjWrap(compoundObjWrap), m_otherObjWrap(otherObjWrap), m_dispatcher(dispatcher), m_dispatchInfo(dispatchInfo), m_resultOut(resultOut), m_childCollisionAlgorithms(childCollisionAlgorithms), m_sharedManifold(sharedManifold)
+	{
 	}
 
-
-	void	ProcessChildShape(const btCollisionShape* childShape,int index)
+	void ProcessChildShape(const btCollisionShape* childShape, int index)
 	{
-		btAssert(index>=0);
+		btAssert(index >= 0);
 		const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape());
-		btAssert(index<compoundShape->getNumChildShapes());
+		btAssert(index < compoundShape->getNumChildShapes());
 
+		if (gCompoundChildShapePairCallback)
+		{
+			if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape))
+				return;
+		}
 
 		//backup
-		btTransform	orgTrans = m_compoundColObjWrap->getWorldTransform();
-		
+		btTransform orgTrans = m_compoundColObjWrap->getWorldTransform();
+
 		const btTransform& childTrans = compoundShape->getChildTransform(index);
-		btTransform	newChildWorldTrans = orgTrans*childTrans ;
+		btTransform newChildWorldTrans = orgTrans * childTrans;
 
 		//perform an AABB check first
-		btVector3 aabbMin0,aabbMax0;
-		childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
+		btVector3 aabbMin0, aabbMax0;
+		childShape->getAabb(newChildWorldTrans, aabbMin0, aabbMax0);
 
 		btVector3 extendAabb(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
 		aabbMin0 -= extendAabb;
 		aabbMax0 += extendAabb;
 
 		btVector3 aabbMin1, aabbMax1;
-		m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1);
+		m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1);
 
-		if (gCompoundChildShapePairCallback)
-		{
-			if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape))
-				return;
-		}
 
-		if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+		if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
 		{
+			btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, -1, index);
 
-			btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index);
-			
 			btCollisionAlgorithm* algo = 0;
 			bool allocatedAlgorithm = false;
 
@@ -172,7 +158,7 @@ public:
 				}
 				algo = m_childCollisionAlgorithms[index];
 			}
-			
+
 			const btCollisionObjectWrapper* tmpWrap = 0;
 
 			///detect swapping case
@@ -180,15 +166,16 @@ public:
 			{
 				tmpWrap = m_resultOut->getBody0Wrap();
 				m_resultOut->setBody0Wrap(&compoundWrap);
-				m_resultOut->setShapeIdentifiersA(-1,index);
-			} else
+				m_resultOut->setShapeIdentifiersA(-1, index);
+			}
+			else
 			{
 				tmpWrap = m_resultOut->getBody1Wrap();
 				m_resultOut->setBody1Wrap(&compoundWrap);
-				m_resultOut->setShapeIdentifiersB(-1,index);
+				m_resultOut->setShapeIdentifiersB(-1, index);
 			}
 
-			algo->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut);
+			algo->processCollision(&compoundWrap, m_otherObjWrap, m_dispatchInfo, m_resultOut);
 
 #if 0
 			if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
@@ -202,18 +189,19 @@ public:
 			if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject())
 			{
 				m_resultOut->setBody0Wrap(tmpWrap);
-			} else
+			}
+			else
 			{
 				m_resultOut->setBody1Wrap(tmpWrap);
 			}
-			if(allocatedAlgorithm)
+			if (allocatedAlgorithm)
 			{
 				algo->~btCollisionAlgorithm();
 				m_dispatcher->freeCollisionAlgorithm(algo);
- 			}
+			}
 		}
 	}
-	void		Process(const btDbvtNode* leaf)
+	void Process(const btDbvtNode* leaf)
 	{
 		int index = leaf->dataAsInt;
 
@@ -230,22 +218,16 @@ public:
 		}
 #endif
 
-		ProcessChildShape(childShape,index);
-
+		ProcessChildShape(childShape, index);
 	}
 };
 
-
-
-
-
-
-void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btCompoundCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
-	const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap;
-	const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap;
+	const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
+	const btCollisionObjectWrapper* otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
 
-	btAssert (colObjWrap->getCollisionShape()->isCompound());
+	btAssert(colObjWrap->getCollisionShape()->isCompound());
 	const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape());
 
 	///btCompoundShape might have changed:
@@ -254,17 +236,17 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 	{
 		///clear and update all
 		removeChildAlgorithms();
-		
-		preallocateChildAlgorithms(body0Wrap,body1Wrap);
+
+		preallocateChildAlgorithms(body0Wrap, body1Wrap);
 		m_compoundShapeRevision = compoundShape->getUpdateRevision();
 	}
 
-	if (m_childCollisionAlgorithms.size()==0)
+	if (m_childCollisionAlgorithms.size() == 0)
 		return;
 
 	const btDbvt* tree = compoundShape->getDynamicAabbTree();
 	//use a dynamic aabb tree to cull potential child-overlaps
-	btCompoundLeafCallback  callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
+	btCompoundLeafCallback callback(colObjWrap, otherObjWrap, m_dispatcher, dispatchInfo, resultOut, &m_childCollisionAlgorithms[0], m_sharedManifold);
 
 	///we need to refresh all contact manifolds
 	///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
@@ -272,18 +254,18 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 	{
 		int i;
 		manifoldArray.resize(0);
-		for (i=0;i<m_childCollisionAlgorithms.size();i++)
+		for (i = 0; i < m_childCollisionAlgorithms.size(); i++)
 		{
 			if (m_childCollisionAlgorithms[i])
 			{
 				m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
-				for (int m=0;m<manifoldArray.size();m++)
+				for (int m = 0; m < manifoldArray.size(); m++)
 				{
 					if (manifoldArray[m]->getNumContacts())
 					{
 						resultOut->setPersistentManifold(manifoldArray[m]);
 						resultOut->refreshContactPoints();
-						resultOut->setPersistentManifold(0);//??necessary?
+						resultOut->setPersistentManifold(0);  //??necessary?
 					}
 				}
 				manifoldArray.resize(0);
@@ -293,57 +275,56 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 
 	if (tree)
 	{
-
-		btVector3 localAabbMin,localAabbMax;
+		btVector3 localAabbMin, localAabbMax;
 		btTransform otherInCompoundSpace;
 		otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform();
-		otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax);
+		otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace, localAabbMin, localAabbMax);
 		btVector3 extraExtends(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold);
 		localAabbMin -= extraExtends;
 		localAabbMax += extraExtends;
 
-		const ATTRIBUTE_ALIGNED16(btDbvtVolume)	bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
+		const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax);
 		//process all children, that overlap with  the given AABB bounds
-		tree->collideTVNoStackAlloc(tree->m_root,bounds,stack2,callback);
-
-	} else
+		tree->collideTVNoStackAlloc(tree->m_root, bounds, stack2, callback);
+	}
+	else
 	{
 		//iterate over all children, perform an AABB check inside ProcessChildShape
 		int numChildren = m_childCollisionAlgorithms.size();
 		int i;
-		for (i=0;i<numChildren;i++)
+		for (i = 0; i < numChildren; i++)
 		{
-			callback.ProcessChildShape(compoundShape->getChildShape(i),i);
+			callback.ProcessChildShape(compoundShape->getChildShape(i), i);
 		}
 	}
 
 	{
-				//iterate over all children, perform an AABB check inside ProcessChildShape
+		//iterate over all children, perform an AABB check inside ProcessChildShape
 		int numChildren = m_childCollisionAlgorithms.size();
 		int i;
 		manifoldArray.resize(0);
-        const btCollisionShape* childShape = 0;
-        btTransform	orgTrans;
-        
-        btTransform	newChildWorldTrans;
-        btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;        
-        
-		for (i=0;i<numChildren;i++)
+		const btCollisionShape* childShape = 0;
+		btTransform orgTrans;
+
+		btTransform newChildWorldTrans;
+		btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1;
+
+		for (i = 0; i < numChildren; i++)
 		{
 			if (m_childCollisionAlgorithms[i])
 			{
 				childShape = compoundShape->getChildShape(i);
-			//if not longer overlapping, remove the algorithm
+				//if not longer overlapping, remove the algorithm
 				orgTrans = colObjWrap->getWorldTransform();
-                
+
 				const btTransform& childTrans = compoundShape->getChildTransform(i);
-                newChildWorldTrans = orgTrans*childTrans ;
+				newChildWorldTrans = orgTrans * childTrans;
 
 				//perform an AABB check first
-				childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
-				otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1);
+				childShape->getAabb(newChildWorldTrans, aabbMin0, aabbMax0);
+				otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1);
 
-				if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1))
+				if (!TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
 				{
 					m_childCollisionAlgorithms[i]->~btCollisionAlgorithm();
 					m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
@@ -354,15 +335,15 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 	}
 }
 
-btScalar	btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 {
 	btAssert(0);
 	//needs to be fixed, using btCollisionObjectWrapper and NOT modifying internal data structures
-	btCollisionObject* colObj = m_isSwapped? body1 : body0;
-	btCollisionObject* otherObj = m_isSwapped? body0 : body1;
+	btCollisionObject* colObj = m_isSwapped ? body1 : body0;
+	btCollisionObject* otherObj = m_isSwapped ? body0 : body1;
+
+	btAssert(colObj->getCollisionShape()->isCompound());
 
-	btAssert (colObj->getCollisionShape()->isCompound());
-	
 	btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
 
 	//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
@@ -376,33 +357,29 @@ btScalar	btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject*
 
 	int numChildren = m_childCollisionAlgorithms.size();
 	int i;
-    btTransform	orgTrans;
-    btScalar frac;
-	for (i=0;i<numChildren;i++)
+	btTransform orgTrans;
+	btScalar frac;
+	for (i = 0; i < numChildren; i++)
 	{
 		//btCollisionShape* childShape = compoundShape->getChildShape(i);
 
 		//backup
-        orgTrans = colObj->getWorldTransform();
-	
+		orgTrans = colObj->getWorldTransform();
+
 		const btTransform& childTrans = compoundShape->getChildTransform(i);
 		//btTransform	newChildWorldTrans = orgTrans*childTrans ;
-		colObj->setWorldTransform( orgTrans*childTrans );
+		colObj->setWorldTransform(orgTrans * childTrans);
 
 		//btCollisionShape* tmpShape = colObj->getCollisionShape();
 		//colObj->internalSetTemporaryCollisionShape( childShape );
-        frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut);
-		if (frac<hitFraction)
+		frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj, otherObj, dispatchInfo, resultOut);
+		if (frac < hitFraction)
 		{
 			hitFraction = frac;
 		}
 		//revert back
 		//colObj->internalSetTemporaryCollisionShape( tmpShape);
-		colObj->setWorldTransform( orgTrans);
+		colObj->setWorldTransform(orgTrans);
 	}
 	return hitFraction;
-
 }
-
-
-