From 8e9181f85c2c78cf4e1de6d71d425386efba9c34 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 16 Jan 2017 08:23:49 -0800 Subject: [PATCH] [MJCF import] add custom broadphase collision filter, for MJCF/MuJoCo compatibility [MJCF import] improve MuJoCo importer, support collision filters fixed a few more warnings --- .../CommonInterfaces/CommonMultiBodyBase.h | 58 +++++++++++++++- .../ImportMJCFDemo/BulletMJCFImporter.cpp | 4 +- .../ImportMJCFDemo/ImportMJCFSetup.cpp | 10 ++- examples/OpenGLWindow/SimpleOpenGL3App.cpp | 6 +- examples/RoboticsLearning/b3RobotSimAPI.cpp | 6 +- .../PhysicsServerCommandProcessor.cpp | 66 ++++++++++++++++++- .../SharedMemory/PhysicsServerExample.cpp | 25 +++---- .../btOverlappingPairCache.h | 5 +- 8 files changed, 156 insertions(+), 24 deletions(-) diff --git a/examples/CommonInterfaces/CommonMultiBodyBase.h b/examples/CommonInterfaces/CommonMultiBodyBase.h index 48130b2d1..54db4598d 100644 --- a/examples/CommonInterfaces/CommonMultiBodyBase.h +++ b/examples/CommonInterfaces/CommonMultiBodyBase.h @@ -18,10 +18,49 @@ #include "CommonWindowInterface.h" #include "CommonCameraInterface.h" +enum MyFilterModes +{ + FILTER_GROUPAMASKB_AND_GROUPBMASKA2=0, + FILTER_GROUPAMASKB_OR_GROUPBMASKA2 +}; + +struct MyOverlapFilterCallback2 : public btOverlapFilterCallback +{ + int m_filterMode; + + MyOverlapFilterCallback2() + :m_filterMode(FILTER_GROUPAMASKB_AND_GROUPBMASKA2) + { + } + + virtual ~MyOverlapFilterCallback2() + {} + // return true when pairs need collision + virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const + { + if (m_filterMode==FILTER_GROUPAMASKB_AND_GROUPBMASKA2) + { + 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_GROUPBMASKA2) + { + bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; + collides = collides || (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); + return collides; + } + return false; + } +}; + struct CommonMultiBodyBase : public CommonExampleInterface { //keep the collision shapes, for deletion/cleanup btAlignedObjectArray m_collisionShapes; + MyOverlapFilterCallback2* m_filterCallback; + btOverlappingPairCache* m_pairCache; btBroadphaseInterface* m_broadphase; btCollisionDispatcher* m_dispatcher; btMultiBodyConstraintSolver* m_solver; @@ -41,7 +80,9 @@ struct CommonMultiBodyBase : public CommonExampleInterface struct GUIHelperInterface* m_guiHelper; CommonMultiBodyBase(GUIHelperInterface* helper) - :m_broadphase(0), + :m_filterCallback(0), + m_pairCache(0), + m_broadphase(0), m_dispatcher(0), m_solver(0), m_collisionConfiguration(0), @@ -59,11 +100,16 @@ struct CommonMultiBodyBase : public CommonExampleInterface ///collision configuration contains default setup for memory, collision setup m_collisionConfiguration = new btDefaultCollisionConfiguration(); //m_collisionConfiguration->setConvexConvexMultipointIterations(); - + m_filterCallback = new MyOverlapFilterCallback2(); + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); - m_broadphase = new btDbvtBroadphase();//btSimpleBroadphase(); + m_pairCache = new btHashedOverlappingPairCache(); + + m_pairCache->setOverlapFilterCallback(m_filterCallback); + + m_broadphase = new btDbvtBroadphase(m_pairCache);//btSimpleBroadphase(); m_solver = new btMultiBodyConstraintSolver; @@ -142,7 +188,13 @@ struct CommonMultiBodyBase : public CommonExampleInterface delete m_dispatcher; m_dispatcher=0; + + delete m_pairCache; + m_pairCache = 0; + delete m_filterCallback; + m_filterCallback = 0; + delete m_collisionConfiguration; m_collisionConfiguration=0; } diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index 3b06259f9..9599b7fbb 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -1137,7 +1137,7 @@ bool BulletMJCFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const int BulletMJCFImporter::getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const { int flags = 0; - /* + const UrdfLink* link = m_data->getLink(m_data->m_activeModel,linkIndex); if (link) { @@ -1151,7 +1151,7 @@ int BulletMJCFImporter::getCollisionGroupAndMask(int linkIndex, int& colGroup, i } } - */ + return flags; } diff --git a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp index 481c0a1c3..2008d52e9 100644 --- a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp +++ b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp @@ -51,6 +51,8 @@ static btAlignedObjectArray gMCFJFileNameArray; #define MAX_NUM_MOTORS 1024 + + struct ImportMJCFInternalData { ImportMJCFInternalData() @@ -171,13 +173,19 @@ struct MyMJCFLogger : public MJCFErrorLogger } }; + void ImportMJCFSetup::initPhysics() { m_guiHelper->setUpAxis(m_upAxis); - this->createEmptyDynamicsWorld(); + createEmptyDynamicsWorld(); + + //MuJoCo uses a slightly different collision filter mode, use the FILTER_GROUPAMASKB_OR_GROUPBMASKA2 + //@todo also use the modified collision filter for raycast and other collision related queries + m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2; + //m_dynamicsWorld->getSolverInfo().m_numIterations = 100; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->getDebugDrawer()->setDebugMode( diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index de192c5c1..c8d8f2a45 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -672,13 +672,15 @@ void SimpleOpenGL3App::getScreenPixels(unsigned char* rgbaBuffer, int bufferSize if ((width*height*4) == bufferSizeInBytes) { glReadPixels(0,0,width, height, GL_RGBA, GL_UNSIGNED_BYTE, rgbaBuffer); - int glstat = glGetError(); + int glstat; + glstat = glGetError(); b3Assert(glstat==GL_NO_ERROR); } if ((width*height*sizeof(float)) == depthBufferSizeInBytes) { glReadPixels(0,0,width, height, GL_DEPTH_COMPONENT, GL_FLOAT, depthBuffer); - int glstat = glGetError(); + int glstat; + glstat = glGetError(); b3Assert(glstat==GL_NO_ERROR); } diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index bf6403d2e..3ac3c09a7 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -616,7 +616,8 @@ void b3RobotSimAPI::processMultiThreadedGraphicsRequests() case eRobotSimGUIHelperRemoveAllGraphicsInstances: { m_data->m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances(); - int numRenderInstances = m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances(); + int numRenderInstances; + numRenderInstances = m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances(); b3Assert(numRenderInstances==0); m_data->m_multiThreadedHelper->getCriticalSection()->lock(); @@ -901,7 +902,8 @@ bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper) m_data->m_args[0].m_cs->setSharedParam(1, eRobotSimGUIHelperIdle); m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs); - bool serverConnected = m_data->m_physicsServer.connectSharedMemory(m_data->m_multiThreadedHelper); + bool serverConnected; + serverConnected = m_data->m_physicsServer.connectSharedMemory(m_data->m_multiThreadedHelper); b3Assert(serverConnected); m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b14090b8d..2c404407e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -376,6 +376,47 @@ 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) + { + } + + virtual ~MyOverlapFilterCallback() + {} + // return true when pairs need collision + virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const + { + if (m_filterMode==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; + } + return false; + } +}; + + + + struct PhysicsServerCommandProcessorInternalData { ///handle management @@ -501,6 +542,9 @@ struct PhysicsServerCommandProcessorInternalData btAlignedObjectArray m_strings; btAlignedObjectArray m_collisionShapes; + + MyOverlapFilterCallback* m_broadphaseCollisionFilterCallback; + btHashedOverlappingPairCache* m_pairCache; btBroadphaseInterface* m_broadphase; btCollisionDispatcher* m_dispatcher; btMultiBodyConstraintSolver* m_solver; @@ -559,6 +603,12 @@ struct PhysicsServerCommandProcessorInternalData m_physicsDeltaTime(1./240.), m_numSimulationSubSteps(0), m_userConstraintUIDGenerator(1), + m_broadphaseCollisionFilterCallback(0), + m_pairCache(0), + m_broadphase(0), + m_dispatcher(0), + m_solver(0), + m_collisionConfiguration(0), m_dynamicsWorld(0), m_remoteDebugDrawer(0), m_guiHelper(0), @@ -698,7 +748,6 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor() } - void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() { ///collision configuration contains default setup for memory, collision setup @@ -711,6 +760,14 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) 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_pairCache = new btHashedOverlappingPairCache(); + + m_data->m_pairCache->setOverlapFilterCallback(m_data->m_broadphaseCollisionFilterCallback); + m_data->m_broadphase = new btDbvtBroadphase(); m_data->m_solver = new btMultiBodyConstraintSolver; @@ -865,9 +922,16 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() delete m_data->m_solver; m_data->m_solver=0; + delete m_data->m_broadphase; m_data->m_broadphase=0; + delete m_data->m_pairCache; + m_data->m_pairCache= 0; + + delete m_data->m_broadphaseCollisionFilterCallback; + m_data->m_broadphaseCollisionFilterCallback= 0; + delete m_data->m_dispatcher; m_data->m_dispatcher=0; diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 0641389f7..3a850a5b1 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1368,7 +1368,8 @@ void PhysicsServerExample::stepSimulation(float deltaTime) m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances(); if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()) { - int numRenderInstances = m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances(); + int numRenderInstances; + numRenderInstances = m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances(); b3Assert(numRenderInstances==0); } m_multiThreadedHelper->mainThreadRelease(); @@ -1379,17 +1380,17 @@ void PhysicsServerExample::stepSimulation(float deltaTime) case eGUIHelperCopyCameraImageData: { m_multiThreadedHelper->m_childGuiHelper->copyCameraImageData(m_multiThreadedHelper->m_viewMatrix, - m_multiThreadedHelper->m_projectionMatrix, - m_multiThreadedHelper->m_pixelsRGBA, - m_multiThreadedHelper->m_rgbaBufferSizeInPixels, - m_multiThreadedHelper->m_depthBuffer, - m_multiThreadedHelper->m_depthBufferSizeInPixels, - m_multiThreadedHelper->m_segmentationMaskBuffer, - m_multiThreadedHelper->m_segmentationMaskBufferSizeInPixels, - m_multiThreadedHelper->m_startPixelIndex, - m_multiThreadedHelper->m_destinationWidth, - m_multiThreadedHelper->m_destinationHeight, - m_multiThreadedHelper->m_numPixelsCopied); + m_multiThreadedHelper->m_projectionMatrix, + m_multiThreadedHelper->m_pixelsRGBA, + m_multiThreadedHelper->m_rgbaBufferSizeInPixels, + m_multiThreadedHelper->m_depthBuffer, + m_multiThreadedHelper->m_depthBufferSizeInPixels, + m_multiThreadedHelper->m_segmentationMaskBuffer, + m_multiThreadedHelper->m_segmentationMaskBufferSizeInPixels, + m_multiThreadedHelper->m_startPixelIndex, + m_multiThreadedHelper->m_destinationWidth, + m_multiThreadedHelper->m_destinationHeight, + m_multiThreadedHelper->m_numPixelsCopied); m_multiThreadedHelper->mainThreadRelease(); break; } diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index 146142704..6488bdd64 100644 --- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -90,7 +90,8 @@ public: }; /// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com -class btHashedOverlappingPairCache : public btOverlappingPairCache + +ATTRIBUTE_ALIGNED16(class) btHashedOverlappingPairCache : public btOverlappingPairCache { btBroadphasePairArray m_overlappingPairArray; btOverlapFilterCallback* m_overlapFilterCallback; @@ -103,6 +104,8 @@ protected: public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + btHashedOverlappingPairCache(); virtual ~btHashedOverlappingPairCache();