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

(even when sleeping is disabled, islands are split)

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -610,9 +610,10 @@ void ConvertURDF2BulletInternal(
 					}
                 } else
                 {
-					if (canSleep)
+//					if (canSleep)
 					{
-						if (cache.m_bulletMultiBody->getBaseMass()==0 && cache.m_bulletMultiBody->getNumDofs()==0)
+						if (cache.m_bulletMultiBody->getBaseMass()==0)
+							//&& cache.m_bulletMultiBody->getNumDofs()==0)
 						{
 							//col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
 							col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -629,6 +629,15 @@ B3_SHARED_API    int b3PhysicsParameterSetConstraintSolverType(b3SharedMemoryCom
     return 0;
 }
 
+B3_SHARED_API    int b3PhysicsParameterSetMinimumSolverIslandSize(b3SharedMemoryCommandHandle commandHandle, int minimumSolverIslandSize)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+	command->m_physSimParamArgs.m_minimumSolverIslandSize = minimumSolverIslandSize;
+	command->m_updateFlags |= SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE;
+	return 0;
+}
+
 
 B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode)
 {

examples/SharedMemory/PhysicsClientC_API.h

@@ -320,7 +320,8 @@ B3_SHARED_API	int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommand
 B3_SHARED_API	int b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle commandHandle, double contactSlop);
 B3_SHARED_API	int b3PhysicsParameterSetEnableSAT(b3SharedMemoryCommandHandle commandHandle, int enableSAT);
 B3_SHARED_API    int b3PhysicsParameterSetConstraintSolverType(b3SharedMemoryCommandHandle commandHandle, int constraintSolverType);
-    
+B3_SHARED_API    int b3PhysicsParameterSetMinimumSolverIslandSize(b3SharedMemoryCommandHandle commandHandle, int minimumSolverIslandSize);
+
     
 
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -2390,6 +2390,8 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 	m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = 0.2;//need to check if there are artifacts with frictionERP
 	m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
 	m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
+	m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 0;
+	gDbvtMargin = btScalar(0.001);//1mm
 	m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
 //	m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 2;
 	//todo: islands/constraints are buggy in btMultiBodyDynamicsWorld! (performance + see slipping grasp)

examples/SharedMemory/SharedMemoryCommands.h

@@ -468,6 +468,8 @@ enum EnumSimParamUpdateFlags
 	SIM_PARAM_UPDATE_CONTACT_SLOP = 4194304,
 	SIM_PARAM_ENABLE_SAT = 8388608,
     SIM_PARAM_CONSTRAINT_SOLVER_TYPE =16777216,
+	SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE = 33554432,
+
 };
 
 enum EnumLoadSoftBodyUpdateFlags

examples/SharedMemory/SharedMemoryPublic.h

@@ -898,6 +898,7 @@ struct b3PhysicsSimulationParameters
 	double m_contactSlop;
 	int m_enableSAT;
     int m_constraintSolverType;
+	int m_minimumSolverIslandSize;
 };
 
 enum eConstraintSolverTypes

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp

@@ -1353,6 +1353,10 @@ bool b3RobotSimulatorClientAPI_NoDirect::changeDynamics(int bodyUniqueId, int li
 	b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(sm);
 	b3SharedMemoryStatusHandle statusHandle;
 
+	if (args.m_activationState >= 0)
+	{
+		b3ChangeDynamicsInfoSetActivationState(command, bodyUniqueId,args.m_activationState);
+	}
 	if (args.m_mass >= 0) {
 		b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, args.m_mass);
 	}
@@ -1714,6 +1718,15 @@ bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(const struct
 		b3PhysicsParamSetSolverResidualThreshold(command, args.m_solverResidualThreshold);
 	}
 
+	if (args.m_constraintSolverType >= 0)
+	{
+		b3PhysicsParameterSetConstraintSolverType(command, args.m_constraintSolverType);
+	}
+	if (args.m_minimumSolverIslandSize >= 0)
+	{
+		b3PhysicsParameterSetMinimumSolverIslandSize(command, args.m_minimumSolverIslandSize);
+	}
+
 	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
 	return true;
 }

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h

@@ -268,7 +268,8 @@ struct b3RobotSimulatorSetPhysicsEngineParameters : b3PhysicsSimulationParameter
 		
 		m_frictionERP=-1;
 	  	m_solverResidualThreshold=-1;
-
+		m_constraintSolverType = -1;
+		m_minimumSolverIslandSize = -1;
 		
 	}
 };
@@ -285,6 +286,7 @@ struct b3RobotSimulatorChangeDynamicsArgs
 	double m_contactStiffness;
 	double m_contactDamping;
 	int m_frictionAnchor;
+	int m_activationState;
 
 	b3RobotSimulatorChangeDynamicsArgs()
 		: m_mass(-1),
@@ -296,7 +298,8 @@ struct b3RobotSimulatorChangeDynamicsArgs
 		m_angularDamping(-1),
 		m_contactStiffness(-1),
 		m_contactDamping(-1),
-		m_frictionAnchor(-1)
+		m_frictionAnchor(-1),
+		m_activationState(-1)
 	{}
 };
 

examples/pybullet/pybullet.c

@@ -1469,12 +1469,14 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
     int constraintSolverType=-1;
     double globalCFM = -1;
     
+	int minimumSolverIslandSize = -1;
+	
 
 	int physicsClientId = 0;
-	static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "enableSAT", "constraintSolverType", "globalCFM", "physicsClientId", NULL};
+	static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "enableSAT", "constraintSolverType", "globalCFM", "minimumSolverIslandSize", "physicsClientId", NULL};
 
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiiddi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
-									 &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &physicsClientId))
+									 &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -1495,6 +1497,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 			b3PhysicsParamSetNumSolverIterations(command, numSolverIterations);
 		}
 
+		if (minimumSolverIslandSize >= 0)
+		{
+			b3PhysicsParameterSetMinimumSolverIslandSize(command, minimumSolverIslandSize);
+		}
+
+
 		if (solverResidualThreshold>=0)
 		{
 			b3PhysicsParamSetSolverResidualThreshold(command, solverResidualThreshold);

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	(
+				if (
-#ifdef DBVT_BP_MARGIN				
+					m_sets[0].update(proxy->leaf, aabb, velocity, gDbvtMargin)
-					m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
+
-#else
-					m_sets[0].update(proxy->leaf,aabb,velocity)
-#endif
 					)
 				{
 					++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 (m_linkA < 0)
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
 		{
-			if (m_bodyA->getLink(i).m_collider)
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			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 (m_linkB < 0)
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
 		{
-			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 (m_linkA < 0)
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
 		{
-			if (m_bodyA->getLink(i).m_collider)
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			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 (m_linkB < 0)
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
 		{
-			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 (m_linkA < 0)
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
 		{
-			if (m_bodyA->getLink(i).m_collider)
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			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 (m_linkB < 0)
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
 		{
-			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 (this->m_linkA < 0)
-	if (col)
-		return col->getIslandTag();
-	for (int i=0;i<m_bodyA->getNumLinks();i++)
 	{
-		if (m_bodyA->getLink(i).m_collider)
+		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-			return m_bodyA->getLink(i).m_collider->getIslandTag();
+		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 (m_linkB < 0)
-	if (col)
-		return col->getIslandTag();
-
-	for (int i=0;i<m_bodyB->getNumLinks();i++)
 	{
-		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 (m_linkA < 0)
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
 		{
-			if (m_bodyA->getLink(i).m_collider)
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			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 (m_linkB < 0)
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
 		{
-			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 (m_linkA < 0)
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
 		{
-			if (m_bodyA->getLink(i).m_collider)
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			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 (m_linkB < 0)
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
 		{
-			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