From 955389277087da0e357299c21956d4bb34967fbf Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 12 Sep 2018 19:30:49 -0700 Subject: [PATCH] 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 . 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 | 29 +++ .../ImportURDFDemo/BulletUrdfImporter.cpp | 26 ++- .../ImportURDFDemo/BulletUrdfImporter.h | 4 +- .../Importers/ImportURDFDemo/UrdfParser.cpp | 18 ++ examples/SharedMemory/PhysicsClientC_API.cpp | 45 ++++ examples/SharedMemory/PhysicsClientC_API.h | 7 + .../PhysicsServerCommandProcessor.cpp | 199 +++++++++++++++--- .../PhysicsServerCommandProcessor.h | 1 + examples/SharedMemory/SharedMemoryCommands.h | 18 ++ examples/SharedMemory/SharedMemoryPublic.h | 1 + examples/SharedMemory/b3PluginManager.cpp | 50 ++++- examples/SharedMemory/b3PluginManager.h | 8 +- examples/SharedMemory/plugins/b3PluginAPI.h | 1 + .../plugins/b3PluginCollisionInterface.h | 33 +++ .../collisionFilterPlugin.cpp | 189 ++++++++++++----- .../collisionFilterPlugin.h | 4 +- examples/pybullet/examples/collisionFilter.py | 17 ++ examples/pybullet/pybullet.c | 84 ++++++++ .../CollisionDispatch/btCollisionWorld.cpp | 27 +++ .../CollisionDispatch/btCollisionWorld.h | 3 + 20 files changed, 675 insertions(+), 89 deletions(-) create mode 100644 data/cube_collisionfilter.urdf create mode 100644 examples/SharedMemory/plugins/b3PluginCollisionInterface.h create mode 100644 examples/pybullet/examples/collisionFilter.py 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;