diff --git a/data/cube_collisionfilter.urdf b/data/cube_collisionfilter.urdf
new file mode 100644
index 000000000..1e4add6fe
--- /dev/null
+++ b/data/cube_collisionfilter.urdf
@@ -0,0 +1,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
index e6e6c0081..11eb06d19 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+++ b/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;vm_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
{
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
index efab36121..b3ba4198a 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
@@ -63,7 +63,7 @@ public:
virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
virtual void getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const;
-
+
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
@@ -81,6 +81,8 @@ public:
///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
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);
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
index b39eff2d8..ede976aa4 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp
+++ b/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(group_char);
+ }
+ }
+
+ {
+ const char* mask_char = config->Attribute("mask");
+ if (mask_char)
+ {
+ collision.m_flags |= URDF_HAS_COLLISION_MASK;
+ collision.m_collisionMask = urdfLexicalCast(mask_char);
+ }
+ }
+
const char *name_char = config->Attribute("name");
if (name_char)
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 8d43b9714..373277fe3 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/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)
{
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index c93bd2a96..964f87c83 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/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);
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 9f09d8dfd..ab9f7fc23 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/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;
-
- MyOverlapFilterCallback()
- :m_filterMode(FILTER_GROUPAMASKB_AND_GROUPBMASKA)
+ b3PluginManager* m_pluginManager;
+
+ MyOverlapFilterCallback(b3PluginManager* pluginManager)
+ :m_filterMode(B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA),
+ m_pluginManager(pluginManager)
{
}
@@ -771,20 +769,60 @@ 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())
{
- bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
- collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
- return collides;
- }
+
+ 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)
- {
- bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
- collides = collides || (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
- return collides;
+ 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);
+ return collides;
+ }
+ return false;
}
- 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->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA;
+ m_data->m_broadphaseCollisionFilterCallback = new MyOverlapFilterCallback(&m_data->m_pluginManager);
+ 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 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;im_multiBody)
+ {
+
+ if (body->m_multiBody->getBaseCollider())
+ {
+ m_data->m_dynamicsWorld->refreshBroadphaseProxy(body->m_multiBody->getBaseCollider());
+ }
+ for (int i=0;im_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;
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h
index 5bfff3e96..44458cfdc 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.h
+++ b/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);
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index dac0415e0..90a79331f 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/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;
};
};
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index f9a7dd2cf..55135dd22 100644
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/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,
diff --git a/examples/SharedMemory/b3PluginManager.cpp b/examples/SharedMemory/b3PluginManager.cpp
index 667aa59d7..2b708cc88 100644
--- a/examples/SharedMemory/b3PluginManager.cpp
+++ b/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 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);
@@ -286,6 +293,17 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
selectPluginRenderer(pluginUniqueId);
}
}
+
+ //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;
+}
+
diff --git a/examples/SharedMemory/b3PluginManager.h b/examples/SharedMemory/b3PluginManager.h
index b1eb592f5..df7d42164 100644
--- a/examples/SharedMemory/b3PluginManager.h
+++ b/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
diff --git a/examples/SharedMemory/plugins/b3PluginAPI.h b/examples/SharedMemory/plugins/b3PluginAPI.h
index 71025e8e1..af81c3a9c 100644
--- a/examples/SharedMemory/plugins/b3PluginAPI.h
+++ b/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
diff --git a/examples/SharedMemory/plugins/b3PluginCollisionInterface.h b/examples/SharedMemory/plugins/b3PluginCollisionInterface.h
new file mode 100644
index 000000000..75d0bb774
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp b/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
index 21fbc8354..4e65cfcbc 100644
--- a/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
+++ b/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
@@ -7,11 +7,148 @@
#include "../../SharedMemoryPublic.h"
#include "../b3PluginContext.h"
#include
+#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 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)
-{
- //apply pd control here, apply forces using the PD gains
- return 0;
-}
-
-
-B3_SHARED_API int postTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context)
+B3_SHARED_API struct b3PluginCollisionInterface* getCollisionInterface_collisionFilterPlugin(struct b3PluginContext* context)
{
CollisionFilterMyClass* obj = (CollisionFilterMyClass* )context->m_userPointer;
- obj->m_testData++;
- return 0;
+ return &obj->m_collisionFilter;
}
+
B3_SHARED_API int executePluginCommand_collisionFilterPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
- //set the PD gains
- printf("text argument:%s\n",arguments->m_text);
- printf("int args: [");
- for (int i=0;im_numInts;i++)
- {
- printf("%d", arguments->m_ints[i]);
- if ((i+1)m_numInts)
- {
- printf(",");
- }
- }
- printf("]\nfloat args: [");
- for (int i=0;im_numFloats;i++)
- {
- printf("%f", arguments->m_floats[i]);
- if ((i+1)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;
+ return 0;
}
diff --git a/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.h b/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.h
index 5fd96fd46..9421bc336 100644
--- a/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.h
+++ b/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 int postTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context);
-
+B3_SHARED_API struct b3PluginCollisionInterface* getCollisionInterface_collisionFilterPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};
diff --git a/examples/pybullet/examples/collisionFilter.py b/examples/pybullet/examples/collisionFilter.py
new file mode 100644
index 000000000..ca7108891
--- /dev/null
+++ b/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)
\ No newline at end of file
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 864c7cdfb..9654d48a7 100644
--- a/examples/pybullet/pybullet.c
+++ b/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."},
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
index 3de8d6995..a16b29539 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -107,7 +107,34 @@ 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)
{
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
index 886476e8a..fd1509359 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
+++ b/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;