Fix some deactivation issues with btMultiBodyDynamicsWorld, should also improve performance for PyBullet with larger worlds

(even when sleeping is disabled, islands are split)

src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp

@@ -17,7 +17,7 @@ subject to the following restrictions:
 
 #include "btDbvtBroadphase.h"
 #include "LinearMath/btThreads.h"
-
+btScalar gDbvtMargin = btScalar(0.05);
 //
 // Profiling
 //
@@ -332,12 +332,9 @@ void							btDbvtBroadphase::setAabb(		btBroadphaseProxy* absproxy,
 				if(delta[0]<0) velocity[0]=-velocity[0];
 				if(delta[1]<0) velocity[1]=-velocity[1];
 				if(delta[2]<0) velocity[2]=-velocity[2];
-				if	(
-#ifdef DBVT_BP_MARGIN				
-					m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
-#else
-					m_sets[0].update(proxy->leaf,aabb,velocity)
-#endif
+				if (
+					m_sets[0].update(proxy->leaf, aabb, velocity, gDbvtMargin)
+
 					)
 				{
 					++m_updates_done;

src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h

@@ -29,7 +29,8 @@ subject to the following restrictions:
 #define DBVT_BP_PREVENTFALSEUPDATE		0
 #define DBVT_BP_ACCURATESLEEPING		0
 #define DBVT_BP_ENABLE_BENCHMARK		0
-#define DBVT_BP_MARGIN					(btScalar)0.05
+//#define DBVT_BP_MARGIN					(btScalar)0.05
+extern btScalar gDbvtMargin;
 
 #if DBVT_BP_PROFILE
 #define	DBVT_BP_PROFILING_RATE	256

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -1406,6 +1406,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
 
 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
 {
+	//printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints);
 	return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
 }
 
@@ -1656,6 +1657,8 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
 
 void  btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
 {
+	//printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints);
+
 	//printf("solveMultiBodyGroup start\n");
 	m_tmpMultiBodyConstraints = multiBodyConstraints;
 	m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -352,7 +352,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 				for (i=0;i<numCurMultiBodyConstraints;i++)
 					m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
 				
-				if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
+				if ((m_multiBodyConstraints.size()+m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
 				{
 					processConstraints();
 				} else

src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp

@@ -65,13 +65,16 @@ int btMultiBodyFixedConstraint::getIslandIdA() const
 
 	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
 		}
 	}
 	return -1;
@@ -83,16 +86,17 @@ int btMultiBodyFixedConstraint::getIslandIdB() const
 		return m_rigidBodyB->getIslandTag();
 	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }

src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp

@@ -45,16 +45,18 @@ btMultiBodyGearConstraint::~btMultiBodyGearConstraint()
 
 int btMultiBodyGearConstraint::getIslandIdA() const
 {
-
 	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
 		}
 	}
 	return -1;
@@ -64,16 +66,17 @@ int btMultiBodyGearConstraint::getIslandIdB() const
 {
 	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp

@@ -53,17 +53,22 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
 {
 }
 
+
 int btMultiBodyJointLimitConstraint::getIslandIdA() const
 {
-	if(m_bodyA)
+
+	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
 		}
 	}
 	return -1;
@@ -71,18 +76,19 @@ int btMultiBodyJointLimitConstraint::getIslandIdA() const
 
 int btMultiBodyJointLimitConstraint::getIslandIdB() const
 {
-	if(m_bodyB)
+	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp

@@ -74,29 +74,37 @@ btMultiBodyJointMotor::~btMultiBodyJointMotor()
 
 int btMultiBodyJointMotor::getIslandIdA() const
 {
-	btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-	if (col)
-		return col->getIslandTag();
-	for (int i=0;i<m_bodyA->getNumLinks();i++)
+	if (this->m_linkA < 0)
 	{
-		if (m_bodyA->getLink(i).m_collider)
-			return m_bodyA->getLink(i).m_collider->getIslandTag();
+		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyA->getLink(m_linkA).m_collider)
+		{
+			return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }
 
 int btMultiBodyJointMotor::getIslandIdB() const
 {
-	btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-	if (col)
-		return col->getIslandTag();
-
-	for (int i=0;i<m_bodyB->getNumLinks();i++)
+	if (m_linkB < 0)
 	{
-		col = m_bodyB->getLink(i).m_collider;
+		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 		if (col)
 			return col->getIslandTag();
 	}
+	else
+	{
+		if (m_bodyB->getLink(m_linkB).m_collider)
+		{
+			return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
 	return -1;
 }
 

src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp

@@ -64,13 +64,16 @@ int btMultiBodyPoint2Point::getIslandIdA() const
 
 	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();			
 		}
 	}
 	return -1;
@@ -82,16 +85,17 @@ int btMultiBodyPoint2Point::getIslandIdB() const
 		return m_rigidBodyB->getIslandTag();
 	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }

src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp

@@ -68,13 +68,16 @@ int btMultiBodySliderConstraint::getIslandIdA() const
 
 	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
 		}
 	}
 	return -1;
@@ -86,20 +89,20 @@ int btMultiBodySliderConstraint::getIslandIdB() const
 		return m_rigidBodyB->getIslandTag();
 	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }
-
 void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
 {
     // Convert local points back to world