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

Update Bullet physics to commit 126b676

1 files changed, 77 insertions, 111 deletions

diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
index f8794dec47..25b2b1ea46 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
@@ -13,8 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #include "btCollisionDispatcher.h"
 #include "LinearMath/btQuickprof.h"
 
@@ -31,40 +29,34 @@ subject to the following restrictions:
 #include <stdio.h>
 #endif
 
-
-btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): 
-m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD),
-	m_collisionConfiguration(collisionConfiguration)
+btCollisionDispatcher::btCollisionDispatcher(btCollisionConfiguration* collisionConfiguration) : m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD),
+																								 m_collisionConfiguration(collisionConfiguration)
 {
 	int i;
 
 	setNearCallback(defaultNearCallback);
-	
+
 	m_collisionAlgorithmPoolAllocator = collisionConfiguration->getCollisionAlgorithmPool();
 
 	m_persistentManifoldPoolAllocator = collisionConfiguration->getPersistentManifoldPool();
 
-	for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++)
+	for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++)
 	{
-		for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
+		for (int j = 0; j < MAX_BROADPHASE_COLLISION_TYPES; j++)
 		{
-			m_doubleDispatchContactPoints[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
+			m_doubleDispatchContactPoints[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i, j);
 			btAssert(m_doubleDispatchContactPoints[i][j]);
 			m_doubleDispatchClosestPoints[i][j] = m_collisionConfiguration->getClosestPointsAlgorithmCreateFunc(i, j);
-
 		}
 	}
-	
-	
 }
 
-
-void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
+void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc)
 {
 	m_doubleDispatchContactPoints[proxyType0][proxyType1] = createFunc;
 }
 
-void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
+void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc)
 {
 	m_doubleDispatchClosestPoints[proxyType0][proxyType1] = createFunc;
 }
@@ -73,35 +65,33 @@ btCollisionDispatcher::~btCollisionDispatcher()
 {
 }
 
-btPersistentManifold*	btCollisionDispatcher::getNewManifold(const btCollisionObject* body0,const btCollisionObject* body1) 
-{ 
+btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1)
+{
 	//btAssert(gNumManifold < 65535);
-	
-
 
 	//optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
-	
-	btScalar contactBreakingThreshold =  (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? 
-		btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold) , body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold))
-		: gContactBreakingThreshold ;
-
-	btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold());
-		
- 	void* mem = m_persistentManifoldPoolAllocator->allocate( sizeof( btPersistentManifold ) );
-    if (NULL == mem)
+
+	btScalar contactBreakingThreshold = (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold), body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold))
+																																: gContactBreakingThreshold;
+
+	btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold());
+
+	void* mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
+	if (NULL == mem)
 	{
-        //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert.
-		if ((m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION)==0)
+		//we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert.
+		if ((m_dispatcherFlags & CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION) == 0)
 		{
-			mem = btAlignedAlloc(sizeof(btPersistentManifold),16);
-		} else
+			mem = btAlignedAlloc(sizeof(btPersistentManifold), 16);
+		}
+		else
 		{
 			btAssert(0);
 			//make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration
 			return 0;
 		}
 	}
-	btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold);
+	btPersistentManifold* manifold = new (mem) btPersistentManifold(body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold);
 	manifold->m_index1a = m_manifoldsPtr.size();
 	m_manifoldsPtr.push_back(manifold);
 
@@ -113,17 +103,14 @@ void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold)
 	manifold->clearManifold();
 }
 
-	
 void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
 {
-	
-
 	//printf("releaseManifold: gNumManifold %d\n",gNumManifold);
 	clearManifold(manifold);
 
 	int findIndex = manifold->m_index1a;
 	btAssert(findIndex < m_manifoldsPtr.size());
-	m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1);
+	m_manifoldsPtr.swap(findIndex, m_manifoldsPtr.size() - 1);
 	m_manifoldsPtr[findIndex]->m_index1a = findIndex;
 	m_manifoldsPtr.pop_back();
 
@@ -131,19 +118,15 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
 	if (m_persistentManifoldPoolAllocator->validPtr(manifold))
 	{
 		m_persistentManifoldPoolAllocator->freeMemory(manifold);
-	} else
+	}
+	else
 	{
 		btAlignedFree(manifold);
 	}
-	
 }
 
-	
-
-
-btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType)
+btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType)
 {
-	
 	btCollisionAlgorithmConstructionInfo ci;
 
 	ci.m_dispatcher1 = this;
@@ -161,21 +144,18 @@ btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObje
 	return algo;
 }
 
-
-
-
-bool	btCollisionDispatcher::needsResponse(const btCollisionObject* body0,const btCollisionObject* body1)
+bool btCollisionDispatcher::needsResponse(const btCollisionObject* body0, const btCollisionObject* body1)
 {
 	//here you can do filtering
-	bool hasResponse = 
+	bool hasResponse =
 		(body0->hasContactResponse() && body1->hasContactResponse());
 	//no response between two static/kinematic bodies:
 	hasResponse = hasResponse &&
-		((!body0->isStaticOrKinematicObject()) ||(! body1->isStaticOrKinematicObject()));
+				  ((!body0->isStaticOrKinematicObject()) || (!body1->isStaticOrKinematicObject()));
 	return hasResponse;
 }
 
-bool	btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const btCollisionObject* body1)
+bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1)
 {
 	btAssert(body0);
 	btAssert(body1);
@@ -192,31 +172,27 @@ bool	btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const
 			printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
 		}
 	}
-#endif //BT_DEBUG
+#endif  //BT_DEBUG
 
 	if ((!body0->isActive()) && (!body1->isActive()))
 		needsCollision = false;
 	else if ((!body0->checkCollideWith(body1)) || (!body1->checkCollideWith(body0)))
 		needsCollision = false;
-	
-	return needsCollision ;
 
+	return needsCollision;
 }
 
-
-
 ///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc)
 ///this is useful for the collision dispatcher.
 class btCollisionPairCallback : public btOverlapCallback
 {
 	const btDispatcherInfo& m_dispatchInfo;
-	btCollisionDispatcher*	m_dispatcher;
+	btCollisionDispatcher* m_dispatcher;
 
 public:
-
-	btCollisionPairCallback(const btDispatcherInfo& dispatchInfo,btCollisionDispatcher*	dispatcher)
-	:m_dispatchInfo(dispatchInfo),
-	m_dispatcher(dispatcher)
+	btCollisionPairCallback(const btDispatcherInfo& dispatchInfo, btCollisionDispatcher* dispatcher)
+		: m_dispatchInfo(dispatchInfo),
+		  m_dispatcher(dispatcher)
 	{
 	}
 
@@ -228,87 +204,76 @@ public:
 	}
 	*/
 
-
 	virtual ~btCollisionPairCallback() {}
 
-
-	virtual bool	processOverlap(btBroadphasePair& pair)
+	virtual bool processOverlap(btBroadphasePair& pair)
 	{
-		(*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo);
+		(*m_dispatcher->getNearCallback())(pair, *m_dispatcher, m_dispatchInfo);
 		return false;
 	}
 };
 
-
-
-
-void	btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) 
+void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher)
 {
 	//m_blockedForChanges = true;
 
-	btCollisionPairCallback	collisionCallback(dispatchInfo,this);
+	btCollisionPairCallback collisionCallback(dispatchInfo, this);
 
-    {
+	{
 		BT_PROFILE("processAllOverlappingPairs");
-		pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher, dispatchInfo);
+		pairCache->processAllOverlappingPairs(&collisionCallback, dispatcher, dispatchInfo);
 	}
 
 	//m_blockedForChanges = false;
-
 }
 
-
-
 //by default, Bullet will use this near callback
 void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo)
 {
-		btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
-		btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
+	btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
+	btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
+
+	if (dispatcher.needsCollision(colObj0, colObj1))
+	{
+		btCollisionObjectWrapper obj0Wrap(0, colObj0->getCollisionShape(), colObj0, colObj0->getWorldTransform(), -1, -1);
+		btCollisionObjectWrapper obj1Wrap(0, colObj1->getCollisionShape(), colObj1, colObj1->getWorldTransform(), -1, -1);
 
-		if (dispatcher.needsCollision(colObj0,colObj1))
+		//dispatcher will keep algorithms persistent in the collision pair
+		if (!collisionPair.m_algorithm)
 		{
-			btCollisionObjectWrapper obj0Wrap(0,colObj0->getCollisionShape(),colObj0,colObj0->getWorldTransform(),-1,-1);
-			btCollisionObjectWrapper obj1Wrap(0,colObj1->getCollisionShape(),colObj1,colObj1->getWorldTransform(),-1,-1);
+			collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap, &obj1Wrap, 0, BT_CONTACT_POINT_ALGORITHMS);
+		}
 
+		if (collisionPair.m_algorithm)
+		{
+			btManifoldResult contactPointResult(&obj0Wrap, &obj1Wrap);
 
-			//dispatcher will keep algorithms persistent in the collision pair
-			if (!collisionPair.m_algorithm)
+			if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE)
 			{
-				collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap,0, BT_CONTACT_POINT_ALGORITHMS);
-			}
+				//discrete collision detection query
 
-			if (collisionPair.m_algorithm)
+				collisionPair.m_algorithm->processCollision(&obj0Wrap, &obj1Wrap, dispatchInfo, &contactPointResult);
+			}
+			else
 			{
-				btManifoldResult contactPointResult(&obj0Wrap,&obj1Wrap);
-				
-				if (dispatchInfo.m_dispatchFunc == 		btDispatcherInfo::DISPATCH_DISCRETE)
-				{
-					//discrete collision detection query
-					
-					collisionPair.m_algorithm->processCollision(&obj0Wrap,&obj1Wrap,dispatchInfo,&contactPointResult);
-				} else
-				{
-					//continuous collision detection query, time of impact (toi)
-					btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult);
-					if (dispatchInfo.m_timeOfImpact > toi)
-						dispatchInfo.m_timeOfImpact = toi;
-
-				}
+				//continuous collision detection query, time of impact (toi)
+				btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0, colObj1, dispatchInfo, &contactPointResult);
+				if (dispatchInfo.m_timeOfImpact > toi)
+					dispatchInfo.m_timeOfImpact = toi;
 			}
 		}
-
+	}
 }
 
-
 void* btCollisionDispatcher::allocateCollisionAlgorithm(int size)
 {
-    void* mem = m_collisionAlgorithmPoolAllocator->allocate( size );
-    if (NULL == mem)
-    {
-	    //warn user for overflow?
-	    return btAlignedAlloc(static_cast<size_t>(size), 16);
-    }
-    return mem;
+	void* mem = m_collisionAlgorithmPoolAllocator->allocate(size);
+	if (NULL == mem)
+	{
+		//warn user for overflow?
+		return btAlignedAlloc(static_cast<size_t>(size), 16);
+	}
+	return mem;
 }
 
 void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr)
@@ -316,7 +281,8 @@ void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr)
 	if (m_collisionAlgorithmPoolAllocator->validPtr(ptr))
 	{
 		m_collisionAlgorithmPoolAllocator->freeMemory(ptr);
-	} else
+	}
+	else
 	{
 		btAlignedFree(ptr);
 	}