PyBullet / BulletRobotics:

Implement collisionFilterPlugin, use setCollisionFilterPair to enable or disable collision detection between specific pairs of objects.
Also, expose setCollisionFilterGroupMask as PyBullet API and in urdf using the tag <collision group="1" mask="2"/>.
See examples/pybullet/examples/collisionFilter.py for an example.
PyBullet default: Lower the warmstarting factor, for maximal coordinates rigid bodies for more stable simulation.
Add btCollisionWorld::refreshBroadphaseProxy to easier recreate the broadphase proxy without adding/removing objects to the world.

data/cube_collisionfilter.urdf

@@ -0,0 +1,29 @@
+<?xml version="1.0" ?>
+<robot name="cube">
+  <link name="baseLink">
+     <contact>
+      <lateral_friction value="1"/>
+  </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value="1.0"/>
+       <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="cube.obj" scale="1 1 1"/>
+      </geometry>
+       <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision group="0" mask="0">
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+	 	<box size="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -1405,7 +1405,31 @@ int BulletURDFImporter::getAllocatedTexture(int index) const
 	return m_data->m_allocatedTextures[index];
 }
 
-
+int BulletURDFImporter::getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const
+{
+	int result = 0;
+	UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
+	btAssert(linkPtr);
+	if (linkPtr)
+	{
+		UrdfLink* link = *linkPtr;
+		for (int v=0;v<link->m_collisionArray.size();v++)
+		{
+			const UrdfCollision& col = link->m_collisionArray[v];
+			if (col.m_flags & URDF_HAS_COLLISION_GROUP)
+			{
+				colGroup = col.m_collisionGroup;
+				result |= URDF_HAS_COLLISION_GROUP;
+			}
+			if (col.m_flags & URDF_HAS_COLLISION_MASK)
+			{
+				colMask = col.m_collisionMask;
+				result |= URDF_HAS_COLLISION_MASK;
+			}
+		}
+	}
+	return result;
+}
 
  class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
 {

examples/Importers/ImportURDFDemo/BulletUrdfImporter.h

@@ -82,6 +82,8 @@ public:
     
 	virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
 
+	virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const;
+    
     virtual int getNumAllocatedCollisionShapes() const;
     virtual class btCollisionShape* getAllocatedCollisionShape(int index);
 

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -571,6 +571,24 @@ bool UrdfParser::parseCollision(UrdfCollision& collision, XMLElement* config, Er
 		return false;
 	}
 	
+	{
+		const char* group_char = config->Attribute("group");
+		if (group_char)
+		{
+			collision.m_flags |= URDF_HAS_COLLISION_GROUP;
+			collision.m_collisionGroup = urdfLexicalCast<int>(group_char);
+		}
+	}
+
+	{
+		const char* mask_char = config->Attribute("mask");
+		if (mask_char)
+		{
+			collision.m_flags |= URDF_HAS_COLLISION_MASK;
+			collision.m_collisionMask = urdfLexicalCast<int>(mask_char);
+		}
+	}
+
 	
 	const char *name_char = config->Attribute("name");
 	if (name_char)

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -4303,6 +4303,51 @@ B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3Shar
     return true;
 }
 
+B3_SHARED_API	b3SharedMemoryCommandHandle	b3CollisionFilterCommandInit(b3PhysicsClientHandle physClient)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+	b3Assert(command);
+	command->m_type = CMD_COLLISION_FILTER;
+	command->m_collisionFilterArgs.m_bodyUniqueIdA = -1;
+	command->m_collisionFilterArgs.m_bodyUniqueIdB = -1;
+	command->m_collisionFilterArgs.m_linkIndexA = -2;
+	command->m_collisionFilterArgs.m_linkIndexB = -2;
+	command->m_collisionFilterArgs.m_enableCollision = 0;
+	command->m_updateFlags = 0;
+	return (b3SharedMemoryCommandHandle)command;
+}
+
+B3_SHARED_API	void	b3SetCollisionFilterPair(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA,
+	int bodyUniqueIdB, int linkIndexA, int linkIndexB, int enableCollision)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_COLLISION_FILTER);
+	command->m_updateFlags = B3_COLLISION_FILTER_PAIR;
+	command->m_collisionFilterArgs.m_bodyUniqueIdA = bodyUniqueIdA;
+	command->m_collisionFilterArgs.m_bodyUniqueIdB = bodyUniqueIdB;
+	command->m_collisionFilterArgs.m_linkIndexA = linkIndexA;
+	command->m_collisionFilterArgs.m_linkIndexB = linkIndexB;
+	command->m_collisionFilterArgs.m_enableCollision = enableCollision;
+}
+
+B3_SHARED_API	void	b3SetCollisionFilterGroupMask(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA,
+	int linkIndexA, int collisionFilterGroup, int collisionFilterMask)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_COLLISION_FILTER);
+	command->m_updateFlags = B3_COLLISION_FILTER_GROUP_MASK;
+	command->m_collisionFilterArgs.m_bodyUniqueIdA = bodyUniqueIdA;
+	command->m_collisionFilterArgs.m_linkIndexA = linkIndexA;
+	command->m_collisionFilterArgs.m_collisionFilterGroup = collisionFilterGroup;
+	command->m_collisionFilterArgs.m_collisionFilterMask = collisionFilterMask;
+}
+
+
 ///compute the joint positions to move the end effector to a desired target using inverse kinematics
 B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
 {

examples/SharedMemory/PhysicsClientC_API.h

@@ -431,6 +431,13 @@ B3_SHARED_API	void b3CalculateInverseKinematicsSetMaxNumIterations(b3SharedMemor
 B3_SHARED_API	void b3CalculateInverseKinematicsSetResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double residualThreshold);
 
 
+B3_SHARED_API	b3SharedMemoryCommandHandle	b3CollisionFilterCommandInit(b3PhysicsClientHandle physClient);
+B3_SHARED_API	void	b3SetCollisionFilterPair(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA,
+	int bodyUniqueIdB, int linkIndexA, int linkIndexB, int enableCollision);
+B3_SHARED_API	void	b3SetCollisionFilterGroupMask(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA,
+	int linkIndexA, int collisionFilterGroup, int collisionFilterMask);
+	
+
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3LoadSdfCommandInit2(b3SharedMemoryCommandHandle commandHandle, const char* sdfFileName);
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -1,6 +1,6 @@
 #include "PhysicsServerCommandProcessor.h"
 #include "../CommonInterfaces/CommonRenderInterface.h"
-
+#include "plugins/b3PluginCollisionInterface.h"
 #include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
 #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
 #include "../Importers/ImportURDFDemo/URDF2Bullet.h"
@@ -751,18 +751,16 @@ struct MyBroadphaseCallback : public btBroadphaseAabbCallback
 
 
 
-enum MyFilterModes
+
-{
-	FILTER_GROUPAMASKB_AND_GROUPBMASKA=0,
-	FILTER_GROUPAMASKB_OR_GROUPBMASKA
-};
 
 struct MyOverlapFilterCallback : public btOverlapFilterCallback
 {
 	int m_filterMode;
+	b3PluginManager* m_pluginManager;
 
-	MyOverlapFilterCallback()
+	MyOverlapFilterCallback(b3PluginManager* pluginManager)
-	:m_filterMode(FILTER_GROUPAMASKB_AND_GROUPBMASKA)
+	:m_filterMode(B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA),
+	m_pluginManager(pluginManager)
 	{
 	}
 	
@@ -771,14 +769,53 @@ struct MyOverlapFilterCallback : public btOverlapFilterCallback
 	// return true when pairs need collision
 	virtual bool	needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
 	{
-		if (m_filterMode==FILTER_GROUPAMASKB_AND_GROUPBMASKA)
+		b3PluginCollisionInterface* collisionInterface = m_pluginManager->getCollisionInterface();
+
+		if (collisionInterface && collisionInterface->getNumRules())
+		{
+
+			int objectUniqueIdB=-1, linkIndexB=-1;
+			btCollisionObject* colObjB = (btCollisionObject*)proxy1->m_clientObject;
+			btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(colObjB);
+			if (mblB)
+			{
+				objectUniqueIdB = mblB->m_multiBody->getUserIndex2();
+				linkIndexB = mblB->m_link;
+			} else
+			{
+				objectUniqueIdB = colObjB->getUserIndex2();
+				linkIndexB = -1;
+			}
+			int objectUniqueIdA = -1, linkIndexA = -1;
+			btCollisionObject* colObjA = (btCollisionObject*)proxy0->m_clientObject;
+			btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(colObjA);
+			if (mblA)
+			{
+				objectUniqueIdA = mblA->m_multiBody->getUserIndex2();
+				linkIndexA = mblA->m_link;
+			} else
+			{
+				objectUniqueIdA = colObjA->getUserIndex2();
+				linkIndexA = -1;
+			}
+			int collisionFilterGroupA = proxy0->m_collisionFilterGroup;
+			int collisionFilterMaskA = proxy0->m_collisionFilterMask;
+			int collisionFilterGroupB = proxy1->m_collisionFilterGroup;
+			int collisionFilterMaskB = proxy1->m_collisionFilterMask;
+
+			return collisionInterface->needsBroadphaseCollision(objectUniqueIdA, linkIndexA, 
+					collisionFilterGroupA,collisionFilterMaskA,
+					objectUniqueIdB, linkIndexB,collisionFilterGroupB,collisionFilterMaskB, m_filterMode);
+		} else
+		{
+			if (m_filterMode==B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA)
 			{
 				bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
 				collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
 				return collides;
 			}
 		
-		if (m_filterMode==FILTER_GROUPAMASKB_OR_GROUPBMASKA)
+			if (m_filterMode==B3_FILTER_GROUPAMASKB_OR_GROUPBMASKA)
 			{
 				bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
 				collides = collides || (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
@@ -786,6 +823,7 @@ struct MyOverlapFilterCallback : public btOverlapFilterCallback
 			}
 			return false;
 		}
+	}
 };
 
 
@@ -1698,26 +1736,27 @@ struct PhysicsServerCommandProcessorInternalData
 
 #ifndef SKIP_STATIC_PD_CONTROL_PLUGIN
 		{
-			m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", initPlugin_pdControlPlugin, exitPlugin_pdControlPlugin, executePluginCommand_pdControlPlugin, preTickPluginCallback_pdControlPlugin, 0, 0,0);
+			m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", initPlugin_pdControlPlugin, exitPlugin_pdControlPlugin, executePluginCommand_pdControlPlugin, preTickPluginCallback_pdControlPlugin, 0, 0,0, 0);
 		}
 #endif //SKIP_STATIC_PD_CONTROL_PLUGIN
 
 #ifndef SKIP_COLLISION_FILTER_PLUGIN
 	{
-		m_collisionFilterPlugin = m_pluginManager.registerStaticLinkedPlugin("collisionFilterPlugin", initPlugin_collisionFilterPlugin, exitPlugin_collisionFilterPlugin, executePluginCommand_collisionFilterPlugin, 0,0,0,0);
+		m_collisionFilterPlugin = m_pluginManager.registerStaticLinkedPlugin("collisionFilterPlugin", initPlugin_collisionFilterPlugin, exitPlugin_collisionFilterPlugin, executePluginCommand_collisionFilterPlugin, 0,0,0,0, getCollisionInterface_collisionFilterPlugin);
+		m_pluginManager.selectCollisionPlugin(m_collisionFilterPlugin);
 	}
 #endif
 
 #ifdef ENABLE_STATIC_GRPC_PLUGIN
 	{
-		m_grpcPlugin = m_pluginManager.registerStaticLinkedPlugin("grpcPlugin", initPlugin_grpcPlugin, exitPlugin_grpcPlugin, executePluginCommand_grpcPlugin, 0, 0, 0,processClientCommands_grpcPlugin);
+		m_grpcPlugin = m_pluginManager.registerStaticLinkedPlugin("grpcPlugin", initPlugin_grpcPlugin, exitPlugin_grpcPlugin, executePluginCommand_grpcPlugin, 0, 0, 0,processClientCommands_grpcPlugin,0);
 	}
 #endif //ENABLE_STATIC_GRPC_PLUGIN
 
 #ifdef STATIC_EGLRENDERER_PLUGIN
 	{
 		bool initPlugin = false;
-		int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("eglRendererPlugin", initPlugin_eglRendererPlugin, exitPlugin_eglRendererPlugin, executePluginCommand_eglRendererPlugin,0,0,getRenderInterface_eglRendererPlugin,0, initPlugin);
+		int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("eglRendererPlugin", initPlugin_eglRendererPlugin, exitPlugin_eglRendererPlugin, executePluginCommand_eglRendererPlugin,0,0,getRenderInterface_eglRendererPlugin,0, 0, initPlugin);
 		m_pluginManager.selectPluginRenderer(renderPluginId);
 	}
 #endif//STATIC_EGLRENDERER_PLUGIN
@@ -1727,7 +1766,7 @@ struct PhysicsServerCommandProcessorInternalData
 #ifndef SKIP_STATIC_TINYRENDERER_PLUGIN
 	{
 
-			int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("tinyRendererPlugin", initPlugin_tinyRendererPlugin, exitPlugin_tinyRendererPlugin, executePluginCommand_tinyRendererPlugin,0,0,getRenderInterface_tinyRendererPlugin,0);
+			int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("tinyRendererPlugin", initPlugin_tinyRendererPlugin, exitPlugin_tinyRendererPlugin, executePluginCommand_tinyRendererPlugin,0,0,getRenderInterface_tinyRendererPlugin,0,0);
 			m_pluginManager.selectPluginRenderer(renderPluginId);
 	}
 #endif
@@ -2382,6 +2421,7 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
 };
 
 
+
 void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 {
     m_data->m_constraintSolverType=eConstraintSolverLCP_SI;
@@ -2396,8 +2436,8 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
     m_data->m_dispatcher = new	btCollisionDispatcher(m_data->m_collisionConfiguration);
     
 
-	m_data->m_broadphaseCollisionFilterCallback = new MyOverlapFilterCallback();
+	m_data->m_broadphaseCollisionFilterCallback = new MyOverlapFilterCallback(&m_data->m_pluginManager);
-	m_data->m_broadphaseCollisionFilterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA;
+	m_data->m_broadphaseCollisionFilterCallback->m_filterMode = B3_FILTER_GROUPAMASKB_OR_GROUPBMASKA;
 	
 	m_data->m_pairCache = new btHashedOverlappingPairCache();
 	
@@ -2431,10 +2471,9 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 	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;
+	m_data->m_dynamicsWorld->getSolverInfo().m_warmstartingFactor = 0.1;
 	gDbvtMargin = btScalar(0);
 	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)
 
 	if (m_data->m_guiHelper)
 	{
@@ -5191,6 +5230,97 @@ bool PhysicsServerCommandProcessor::processAddUserDataCommand(const struct Share
 	return hasStatus;
 }
 
+bool PhysicsServerCommandProcessor::processCollisionFilterCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
+{
+	serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
+	b3PluginCollisionInterface* collisionInterface = m_data->m_pluginManager.getCollisionInterface();
+	if (collisionInterface)
+	{
+		if (clientCmd.m_updateFlags & B3_COLLISION_FILTER_PAIR)
+		{
+			collisionInterface ->setBroadphaseCollisionFilter(clientCmd.m_collisionFilterArgs.m_bodyUniqueIdA,
+				clientCmd.m_collisionFilterArgs.m_bodyUniqueIdB,
+				clientCmd.m_collisionFilterArgs.m_linkIndexA,
+				clientCmd.m_collisionFilterArgs.m_linkIndexB,
+				clientCmd.m_collisionFilterArgs.m_enableCollision);
+
+			btAlignedObjectArray<InternalBodyData*> bodies;
+
+			//now also 'refresh' the broadphase collision pairs involved
+			if (clientCmd.m_collisionFilterArgs.m_bodyUniqueIdA>=0)
+			{
+				bodies.push_back(m_data->m_bodyHandles.getHandle(clientCmd.m_collisionFilterArgs.m_bodyUniqueIdA));
+			}
+			if (clientCmd.m_collisionFilterArgs.m_bodyUniqueIdB>=0)
+			{
+				bodies.push_back(m_data->m_bodyHandles.getHandle(clientCmd.m_collisionFilterArgs.m_bodyUniqueIdB));
+			}
+			for (int i=0;i<bodies.size();i++)
+			{
+				InternalBodyData* body = bodies[i];
+				if (body)
+				{
+					if (body->m_multiBody)
+					{
+
+						if (body->m_multiBody->getBaseCollider())
+						{
+							m_data->m_dynamicsWorld->refreshBroadphaseProxy(body->m_multiBody->getBaseCollider());
+						}
+						for (int i=0;i<body->m_multiBody->getNumLinks();i++)
+						{
+							if (body->m_multiBody->getLinkCollider(i))
+							{
+								m_data->m_dynamicsWorld->refreshBroadphaseProxy(body->m_multiBody->getLinkCollider(i));
+							}
+						}
+					}
+					else
+					{
+						//btRigidBody case
+						if (body->m_rigidBody)
+						{
+							m_data->m_dynamicsWorld->refreshBroadphaseProxy(body->m_rigidBody);
+						}
+					}
+				} 
+			}
+		}
+		if (clientCmd.m_updateFlags & B3_COLLISION_FILTER_GROUP_MASK)
+		{
+			InternalBodyData* body = m_data->m_bodyHandles.getHandle(clientCmd.m_collisionFilterArgs.m_bodyUniqueIdA);
+			btCollisionObject* colObj = 0;
+			if (body->m_multiBody)
+			{
+				if (clientCmd.m_collisionFilterArgs.m_linkIndexA)
+				{
+					colObj = body->m_multiBody->getBaseCollider();
+				} else
+				{
+					if (clientCmd.m_collisionFilterArgs.m_linkIndexA>=0 && clientCmd.m_collisionFilterArgs.m_linkIndexA< body->m_multiBody->getNumLinks())
+					{
+						colObj = body->m_multiBody->getLinkCollider(clientCmd.m_collisionFilterArgs.m_linkIndexA);
+					}
+				}
+			} else
+			{
+				if (body->m_rigidBody)
+				{
+					colObj = body->m_rigidBody;
+				}
+			}
+			if (colObj)
+			{
+				colObj->getBroadphaseHandle()->m_collisionFilterGroup = clientCmd.m_collisionFilterArgs.m_collisionFilterGroup;
+				colObj->getBroadphaseHandle()->m_collisionFilterMask = clientCmd.m_collisionFilterArgs.m_collisionFilterMask;
+				m_data->m_dynamicsWorld->refreshBroadphaseProxy(colObj);
+			}
+		}
+	}
+	return true;
+}
+			
+
 bool PhysicsServerCommandProcessor::processRemoveUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
 {
 	bool hasStatus = true;
@@ -10288,6 +10418,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 			hasStatus = processRemoveUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
 			break;
 		}
+	case CMD_COLLISION_FILTER:
+		{
+			hasStatus = processCollisionFilterCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
+			break;
+		}
 	default:
 		{
 			BT_PROFILE("CMD_UNKNOWN");
@@ -10742,6 +10877,12 @@ void PhysicsServerCommandProcessor::resetSimulation()
 		{
 			m_data->m_pluginManager.getRenderInterface()->resetAll();
 		}
+
+		if (m_data->m_pluginManager.getCollisionInterface())
+		{
+			m_data->m_pluginManager.getCollisionInterface()->resetAll();
+		}
+
 		for (int i = 0; i < m_data->m_savedStates.size(); i++)
 		{
 			delete m_data->m_savedStates[i].m_bulletFile;

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -88,6 +88,7 @@ protected:
 	bool processRequestUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processAddUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processRemoveUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+	bool processCollisionFilterCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 
 	int extractCollisionShapes(const class btCollisionShape* colShape, const class btTransform& transform, struct b3CollisionShapeData* collisionShapeBuffer, int maxCollisionShapes);
 

examples/SharedMemory/SharedMemoryCommands.h

@@ -699,6 +699,23 @@ struct CalculateMassMatrixResultArgs
 	int m_dofCount;
 };
 
+enum b3EnumCollisionFilterFlags
+{
+    B3_COLLISION_FILTER_PAIR=1,
+	B3_COLLISION_FILTER_GROUP_MASK=2,
+};
+
+struct b3CollisionFilterArgs
+{
+	int m_bodyUniqueIdA;
+	int m_bodyUniqueIdB;
+	int m_linkIndexA;
+	int m_linkIndexB;
+	int m_enableCollision;
+	int m_collisionFilterGroup;
+	int m_collisionFilterMask;
+};
+
 struct CalculateInverseKinematicsArgs
 {
 	int m_bodyUniqueId;
@@ -1087,6 +1104,7 @@ struct SharedMemoryCommand
 		struct UserDataRequestArgs m_userDataRequestArgs;
 		struct AddUserDataRequestArgs m_addUserDataRequestArgs;
 		struct UserDataRequestArgs m_removeUserDataRequestArgs;
+		struct b3CollisionFilterArgs m_collisionFilterArgs;
     };
 };
 

examples/SharedMemory/SharedMemoryPublic.h

@@ -97,6 +97,7 @@ enum EnumSharedMemoryClientCommand
 	CMD_REQUEST_USER_DATA,
 	CMD_ADD_USER_DATA,
 	CMD_REMOVE_USER_DATA,
+	CMD_COLLISION_FILTER,
 
     //don't go beyond this command!
     CMD_MAX_CLIENT_COMMANDS,

examples/SharedMemory/b3PluginManager.cpp

@@ -47,6 +47,7 @@ struct b3Plugin
 	PFN_TICK m_processClientCommandsFunc;
 
 	PFN_GET_RENDER_INTERFACE m_getRendererFunc;
+	PFN_GET_COLLISION_INTERFACE m_getCollisionFunc;
 
 	void* m_userPointer;
 
@@ -63,6 +64,7 @@ struct b3Plugin
 		m_processNotificationsFunc(0),
 		m_processClientCommandsFunc(0),
 		m_getRendererFunc(0),
+		m_getCollisionFunc(0),
 		m_userPointer(0)
 	{
 	}
@@ -81,6 +83,7 @@ struct b3Plugin
 		m_processNotificationsFunc = 0;
 		m_processClientCommandsFunc = 0;
 		m_getRendererFunc = 0;
+		m_getCollisionFunc = 0;
 		m_userPointer = 0;
 		m_isInitialized = false;
 	}
@@ -100,11 +103,12 @@ struct b3PluginManagerInternalData
 	b3AlignedObjectArray<b3Notification> m_notifications[2];
 	int m_activeNotificationsBufferIndex;
 	int m_activeRendererPluginUid;
+	int m_activeCollisionPluginUid;
 	int m_numNotificationPlugins;
 
 	b3PluginManagerInternalData()
 		:m_rpcCommandProcessorInterface(0), m_activeNotificationsBufferIndex(0), m_activeRendererPluginUid(-1),
-		m_numNotificationPlugins(0)
+		m_activeCollisionPluginUid(-1),	m_numNotificationPlugins(0)
 	{
 	}
 };
@@ -207,6 +211,8 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
 			std::string processNotificationsStr = std::string("processNotifications") + postFix;
 			std::string processClientCommandsStr = std::string("processClientCommands") + postFix;
 			std::string getRendererStr = std::string("getRenderInterface") + postFix;
+			std::string getCollisionStr = std::string("getCollisionInterface") + postFix;
+			
 
 			plugin->m_initFunc = (PFN_INIT)B3_DYNLIB_IMPORT(pluginHandle, initStr.c_str());
 			plugin->m_exitFunc = (PFN_EXIT)B3_DYNLIB_IMPORT(pluginHandle, exitStr.c_str());
@@ -222,6 +228,7 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
 			plugin->m_processClientCommandsFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, processClientCommandsStr.c_str());
 
 			plugin->m_getRendererFunc =  (PFN_GET_RENDER_INTERFACE)B3_DYNLIB_IMPORT(pluginHandle, getRendererStr.c_str());
+			plugin->m_getCollisionFunc = (PFN_GET_COLLISION_INTERFACE)B3_DYNLIB_IMPORT(pluginHandle, getCollisionStr.c_str());
 		
 
 			if (plugin->m_initFunc && plugin->m_exitFunc && plugin->m_executeCommandFunc)
@@ -277,7 +284,7 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
 		}
 	}
 
-	//for now, automatically select the loaded plugin as active renderer. If wanted, we can add some 'select' mechanism.
+	//for now, automatically select the loaded plugin as active renderer.
 	if (pluginUniqueId>=0)
 	{
 		b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId);
@@ -287,6 +294,17 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
 		}
 	}
 	
+	//for now, automatically select the loaded plugin as active collision plugin.
+	if (pluginUniqueId>=0)
+	{
+		b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId);
+		if (plugin && plugin->m_getCollisionFunc)
+		{
+			selectCollisionPlugin(pluginUniqueId);
+		}
+	}
+
+
 	return pluginUniqueId;
 }
 
@@ -433,7 +451,7 @@ int b3PluginManager::executePluginCommand(int pluginUniqueId, const b3PluginArgu
 }
 
 
-int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc,PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, bool initPlugin)
+int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc,PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, PFN_GET_COLLISION_INTERFACE getCollisionFunc, bool initPlugin)
 {
 
 	b3Plugin orgPlugin;
@@ -449,6 +467,7 @@ int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT
 	pluginHandle->m_preTickFunc = preTickFunc;
 	pluginHandle->m_postTickFunc = postTickFunc;
 	pluginHandle->m_getRendererFunc = getRendererFunc;
+	pluginHandle->m_getCollisionFunc = getCollisionFunc;
 	pluginHandle->m_processClientCommandsFunc = processClientCommandsFunc;
 	pluginHandle->m_pluginHandle = 0;
 	pluginHandle->m_pluginPath = pluginPath;
@@ -487,15 +506,36 @@ UrdfRenderingInterface* b3PluginManager::getRenderInterface()
 	if (m_data->m_activeRendererPluginUid>=0)
 	{
 		b3PluginHandle* plugin = m_data->m_plugins.getHandle(m_data->m_activeRendererPluginUid);
-		if (plugin)
+		if (plugin && plugin->m_getRendererFunc)
 		{
 			b3PluginContext context = {0};
 			context.m_userPointer = plugin->m_userPointer;
 			context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
-
 			renderer = plugin->m_getRendererFunc(&context);
 		}
 	}
 	return renderer;
 }
 
+void b3PluginManager::selectCollisionPlugin(int pluginUniqueId)
+{
+	m_data->m_activeCollisionPluginUid = pluginUniqueId;
+}
+
+struct b3PluginCollisionInterface* b3PluginManager::getCollisionInterface()
+{
+	b3PluginCollisionInterface* collisionInterface = 0;
+	if (m_data->m_activeCollisionPluginUid>=0)
+	{
+		b3PluginHandle* plugin = m_data->m_plugins.getHandle(m_data->m_activeCollisionPluginUid);
+		if (plugin && plugin->m_getCollisionFunc)
+		{
+			b3PluginContext context = {0};
+			context.m_userPointer = plugin->m_userPointer;
+			context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
+			collisionInterface = plugin->m_getCollisionFunc(&context);
+		}
+	}
+	return collisionInterface;
+}
+

examples/SharedMemory/b3PluginManager.h

@@ -30,9 +30,13 @@ class b3PluginManager
 		
 		void tickPlugins(double timeStep, b3PluginManagerTickMode tickMode);
 
-		int registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, bool initPlugin=true);
+		int registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, PFN_GET_COLLISION_INTERFACE getCollisionFunc, bool initPlugin=true);
+		
 		void selectPluginRenderer(int pluginUniqueId);
-		UrdfRenderingInterface* getRenderInterface();
+		struct UrdfRenderingInterface* getRenderInterface();
+
+		void selectCollisionPlugin(int pluginUniqueId);
+		struct b3PluginCollisionInterface* getCollisionInterface();
 };
 
 #endif //B3_PLUGIN_MANAGER_H

examples/SharedMemory/plugins/b3PluginAPI.h

@@ -32,6 +32,7 @@ extern "C" {
 	typedef B3_API_ENTRY int (B3_API_CALL * PFN_TICK)(struct b3PluginContext* context);
 
 	typedef B3_API_ENTRY struct UrdfRenderingInterface* (B3_API_CALL * PFN_GET_RENDER_INTERFACE)(struct b3PluginContext* context);
+	typedef B3_API_ENTRY struct b3PluginCollisionInterface* (B3_API_CALL * PFN_GET_COLLISION_INTERFACE)(struct b3PluginContext* context);
 	
 
 #ifdef __cplusplus

examples/SharedMemory/plugins/b3PluginCollisionInterface.h

@@ -0,0 +1,33 @@
+#ifndef B3_PLUGIN_COLLISION_INTERFACE_H
+#define B3_PLUGIN_COLLISION_INTERFACE_H
+
+enum b3PluginCollisionFilterModes
+{
+	B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA=0,
+	B3_FILTER_GROUPAMASKB_OR_GROUPBMASKA
+};
+
+struct b3PluginCollisionInterface
+{
+	virtual void setBroadphaseCollisionFilter(
+		int objectUniqueIdA, int objectUniqueIdB,
+		int linkIndexA, int linkIndexB, 
+		bool enableCollision)=0;
+
+	virtual void removeBroadphaseCollisionFilter(
+		int objectUniqueIdA, int objectUniqueIdB,
+		int linkIndexA, int linkIndexB)=0;
+
+	virtual int getNumRules() const = 0;
+
+	virtual void resetAll()=0;
+
+	virtual int needsBroadphaseCollision(int objectUniqueIdA, int linkIndexA, 
+		int collisionFilterGroupA,int collisionFilterMaskA,  
+		int objectUniqueIdB, int linkIndexB,
+		int collisionFilterGroupB,int collisionFilterMaskB, 
+		int filterMode
+		)=0;
+};
+
+#endif //B3_PLUGIN_COLLISION_INTERFACE_H

examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp

@@ -7,11 +7,148 @@
 #include "../../SharedMemoryPublic.h"
 #include "../b3PluginContext.h"
 #include <stdio.h>
+#include "Bullet3Common/b3HashMap.h"
+
+#include "../b3PluginCollisionInterface.h"
+
+struct b3CustomCollisionFilter
+{
+	int m_objectUniqueIdA;
+	int m_linkIndexA;
+	int m_objectUniqueIdB;
+	int m_linkIndexB;
+	bool m_enableCollision;
+
+	B3_FORCE_INLINE	unsigned int getHash()const
+	{
+		int obA = (m_objectUniqueIdA&0xff);
+		int obB = ((m_objectUniqueIdB &0xf)<<8);
+		int linkA = ((m_linkIndexA&0xff)<<16);
+		int linkB = ((m_linkIndexB&0xff)<<24);
+		int key = obA+obB+linkA+linkB;
+		// Thomas Wang's hash
+		key += ~(key << 15);	key ^=  (key >> 10);	key +=  (key << 3);	key ^=  (key >> 6);	key += ~(key << 11);	key ^=  (key >> 16);
+		return key;
+	}
+	bool equals(const b3CustomCollisionFilter& other) const
+	{
+		return m_objectUniqueIdA == other.m_objectUniqueIdA &&
+			m_objectUniqueIdB == other.m_objectUniqueIdB&&
+			m_linkIndexA == other.m_linkIndexA &&
+			m_linkIndexB == other.m_linkIndexB;
+	}
+	
+};
+
+struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
+{
+	b3HashMap<b3CustomCollisionFilter, b3CustomCollisionFilter> m_customCollisionFilters;
+
+	virtual void setBroadphaseCollisionFilter(
+		int objectUniqueIdA, int objectUniqueIdB,
+		int linkIndexA, int linkIndexB,
+		bool enableCollision)
+	{
+		
+		b3CustomCollisionFilter keyValue;
+		keyValue.m_objectUniqueIdA = objectUniqueIdA;
+		keyValue.m_linkIndexA = linkIndexA;
+		keyValue.m_objectUniqueIdB = objectUniqueIdB;
+		keyValue.m_linkIndexB = linkIndexB;
+		keyValue.m_enableCollision = enableCollision;
+
+		if (objectUniqueIdA>objectUniqueIdB)
+		{
+			b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB);
+			b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
+		}
+
+		m_customCollisionFilters.insert(keyValue,keyValue);
+		
+		
+	}
+
+	virtual void removeBroadphaseCollisionFilter(
+		int objectUniqueIdA, int objectUniqueIdB,
+		int linkIndexA, int linkIndexB)
+	{
+		b3CustomCollisionFilter keyValue;
+		keyValue.m_objectUniqueIdA = objectUniqueIdA;
+		keyValue.m_linkIndexA = linkIndexA;
+		keyValue.m_objectUniqueIdB = objectUniqueIdB;
+		keyValue.m_linkIndexB = linkIndexB;
+		
+		if (objectUniqueIdA>objectUniqueIdB)
+		{
+			b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB);
+			b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
+		}
+
+		m_customCollisionFilters.remove(keyValue);
+	}
+
+	virtual int getNumRules() const
+	{
+		return m_customCollisionFilters.size();
+	}
+
+	virtual void resetAll()
+	{
+		m_customCollisionFilters.clear();
+	}
+
+	virtual int needsBroadphaseCollision(int objectUniqueIdA, int linkIndexA, 
+		int collisionFilterGroupA,int collisionFilterMaskA,  
+		int objectUniqueIdB, int linkIndexB,
+		int collisionFilterGroupB,int collisionFilterMaskB, 
+		int filterMode
+		)
+	{
+		//check and apply any custom rules for those objects/links
+		b3CustomCollisionFilter keyValue;
+		keyValue.m_objectUniqueIdA = objectUniqueIdA;
+		keyValue.m_linkIndexA = linkIndexA;
+		keyValue.m_objectUniqueIdB = objectUniqueIdB;
+		keyValue.m_linkIndexB = linkIndexB;
+
+		if (objectUniqueIdA>objectUniqueIdB)
+		{
+			b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB);
+			b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
+		}
+
+		b3CustomCollisionFilter* filter = m_customCollisionFilters.find(keyValue);
+		if (filter)
+		{
+			return filter->m_enableCollision;
+		}
+
+		//otherwise use the default fallback
+
+		if (filterMode==B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA)
+		{
+			bool collides = (collisionFilterGroupA & collisionFilterMaskB) != 0;
+			collides = collides && (collisionFilterGroupB & collisionFilterMaskA);
+			return collides;
+		}
+		
+		if (filterMode==B3_FILTER_GROUPAMASKB_OR_GROUPBMASKA)
+		{
+			bool collides = (collisionFilterGroupA & collisionFilterMaskB) != 0;
+			collides = collides || (collisionFilterGroupB & collisionFilterMaskA);
+			return collides;
+		}
+		return false;
+		
+	}
+};
 
 struct CollisionFilterMyClass
 {
 	int m_testData;
 
+	DefaultPluginCollisionInterface m_collisionFilter;
+
 	CollisionFilterMyClass()
 		:m_testData(42)
 	{
@@ -29,60 +166,16 @@ B3_SHARED_API int initPlugin_collisionFilterPlugin(struct b3PluginContext* conte
 }
 
 
-B3_SHARED_API int preTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context)
+B3_SHARED_API struct b3PluginCollisionInterface* getCollisionInterface_collisionFilterPlugin(struct b3PluginContext* context)
-{
-	//apply pd control here, apply forces using the PD gains
-	return 0;
-}
-
-
-B3_SHARED_API int postTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context)
 {
 	CollisionFilterMyClass* obj = (CollisionFilterMyClass* )context->m_userPointer;
-	obj->m_testData++;
+	return &obj->m_collisionFilter;
-	return 0;
 }
 
+
 B3_SHARED_API int executePluginCommand_collisionFilterPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
 {
-	//set the PD gains
+	return 0;
-	printf("text argument:%s\n",arguments->m_text);
-	printf("int args: [");
-	for (int i=0;i<arguments->m_numInts;i++)
-	{
-		printf("%d", arguments->m_ints[i]);
-		if ((i+1)<arguments->m_numInts)
-		{
-			printf(",");
-		}
-	}
-	printf("]\nfloat args: [");
-	for (int i=0;i<arguments->m_numFloats;i++)
-	{
-		printf("%f", arguments->m_floats[i]);
-		if ((i+1)<arguments->m_numFloats)
-		{
-			printf(",");
-		}
-	}
-	printf("]\n");
-
-	CollisionFilterMyClass* obj = (CollisionFilterMyClass*) context->m_userPointer;
-	
-	b3SharedMemoryStatusHandle statusHandle;
-	int statusType = -1;
-	int bodyUniqueId = -1;
-
-	b3SharedMemoryCommandHandle command =
-		b3LoadUrdfCommandInit(context->m_physClient, arguments->m_text);
-
-	statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command);
-	statusType = b3GetStatusType(statusHandle);
-	if (statusType == CMD_URDF_LOADING_COMPLETED)
-	{
-		bodyUniqueId = b3GetStatusBodyIndex(statusHandle);	
-	}
-	return bodyUniqueId;
 }
 
 

examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.h

@@ -14,9 +14,7 @@ B3_SHARED_API void exitPlugin_collisionFilterPlugin(struct b3PluginContext* cont
 B3_SHARED_API int executePluginCommand_collisionFilterPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
 
 //all the APIs below are optional
-B3_SHARED_API int preTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context);
+B3_SHARED_API struct b3PluginCollisionInterface* getCollisionInterface_collisionFilterPlugin(struct b3PluginContext* context);
-B3_SHARED_API int postTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context);
-
 
 #ifdef __cplusplus
 };

examples/pybullet/examples/collisionFilter.py

@@ -0,0 +1,17 @@
+import pybullet as p
+import time
+p.connect(p.GUI)
+planeId = p.loadURDF("plane.urdf", useMaximalCoordinates=False)
+cubeId = p.loadURDF("cube_collisionfilter.urdf", [0,0,3], useMaximalCoordinates=False)
+
+collisionFilterGroup = 0
+collisionFilterMask = 0
+p.setCollisionFilterGroupMask(cubeId,-1,collisionFilterGroup,collisionFilterMask)
+
+enableCollision = 1
+p.setCollisionFilterPair(planeId, cubeId,-1,-1,enableCollision )
+
+p.setRealTimeSimulation(1)
+p.setGravity(0,0,-10)
+while (p.isConnected()):
+	time.sleep(1)

examples/pybullet/pybullet.c

@@ -5860,6 +5860,82 @@ static PyObject* MyConvertContactPoint(struct b3ContactInformation* contactPoint
 	return pyResultList;
 }
 
+
+
+static PyObject* pybullet_setCollisionFilterGroupMask(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	int physicsClientId = 0;
+	b3PhysicsClientHandle sm = 0;
+	int bodyUniqueIdA=-1;
+	int linkIndexA =-2;
+	
+	int collisionFilterGroup=-1;
+	int collisionFilterMask = -1;
+
+	b3SharedMemoryCommandHandle	 commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	static char* kwlist[] = {"bodyUniqueId", "linkIndexA","collisionFilterGroup", "collisionFilterMask" , "physicsClientId", NULL};
+
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiii|i", kwlist,
+		&bodyUniqueIdA, &linkIndexA, &collisionFilterGroup, &collisionFilterMask, &physicsClientId))
+		return NULL;
+	
+	commandHandle =	b3CollisionFilterCommandInit(sm);
+	b3SetCollisionFilterGroupMask(commandHandle, bodyUniqueIdA,linkIndexA,collisionFilterGroup, collisionFilterMask);
+	
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	statusType = b3GetStatusType(statusHandle);
+
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
+
+static PyObject* pybullet_setCollisionFilterPair(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	int physicsClientId = 0;
+	b3PhysicsClientHandle sm = 0;
+	int bodyUniqueIdA=-1;
+	int bodyUniqueIdB=-1;
+	int linkIndexA =-2;
+	int linkIndexB =-2;
+	int enableCollision = -1;
+	b3SharedMemoryCommandHandle	 commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	static char* kwlist[] = {"bodyUniqueIdA", "bodyUniqueIdB","linkIndexA","linkIndexB",  "enableCollision", "physicsClientId", NULL};
+
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiiii|i", kwlist,
+		&bodyUniqueIdA, &bodyUniqueIdB,&linkIndexA, &linkIndexB, &enableCollision, &physicsClientId))
+		return NULL;
+	
+	commandHandle =	b3CollisionFilterCommandInit(sm);
+	b3SetCollisionFilterPair(commandHandle, bodyUniqueIdA,bodyUniqueIdB, linkIndexA, linkIndexB, enableCollision);
+	
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	statusType = b3GetStatusType(statusHandle);
+
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
 static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	PyObject *aabbMinOb = 0, *aabbMaxOb = 0;
@@ -9378,6 +9454,14 @@ static PyMethodDef SpamMethods[] = {
 	 "axis-aligned bounding box volume (AABB)."
 	 "Input are two vectors defining the AABB in world space [min_x,min_y,min_z],[max_x,max_y,max_z]."},
 
+	 {"setCollisionFilterPair", (PyCFunction)pybullet_setCollisionFilterPair, METH_VARARGS | METH_KEYWORDS,
+	 "Enable or disable collision detection between two object links."
+	 "Input are two object unique ids and two link indices and an enum"
+	 "to enable or disable collisions."},
+	 
+	 {"setCollisionFilterGroupMask", (PyCFunction)pybullet_setCollisionFilterGroupMask, METH_VARARGS | METH_KEYWORDS,
+	 "Set the collision filter group and the mask for a body."},
+	 
 	{"addUserDebugLine", (PyCFunction)pybullet_addUserDebugLine, METH_VARARGS | METH_KEYWORDS,
 	 "Add a user debug draw line with lineFrom[3], lineTo[3], lineColorRGB[3], lineWidth, lifeTime. "
 	 "A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item."},

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -107,8 +107,35 @@ btCollisionWorld::~btCollisionWorld()
 
 
 
+void	btCollisionWorld::refreshBroadphaseProxy(btCollisionObject* collisionObject)
+{
+	if (collisionObject->getBroadphaseHandle())
+	{
+		int collisionFilterGroup = collisionObject->getBroadphaseHandle()->m_collisionFilterGroup;
+		int collisionFilterMask = collisionObject->getBroadphaseHandle()->m_collisionFilterMask;
 		
 		
+		getBroadphase()->destroyProxy(collisionObject->getBroadphaseHandle(),getDispatcher());
+		
+			//calculate new AABB
+		btTransform trans = collisionObject->getWorldTransform();
+
+		btVector3	minAabb;
+		btVector3	maxAabb;
+		collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
+
+		int type = collisionObject->getCollisionShape()->getShapeType();
+		collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
+			minAabb,
+			maxAabb,
+			type,
+			collisionObject,
+			collisionFilterGroup,
+			collisionFilterMask,
+			m_dispatcher1))	;
+	}
+}
+
 void	btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask)
 {
 

src/BulletCollision/CollisionDispatch/btCollisionWorld.h

@@ -488,6 +488,9 @@ public:
 
 	virtual void	addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter);
 
+	virtual void	refreshBroadphaseProxy(btCollisionObject* collisionObject);
+
+
 	btCollisionObjectArray& getCollisionObjectArray()
 	{
 		return m_collisionObjects;