[MJCF import] add custom broadphase collision filter, for MJCF/MuJoCo compatibility

[MJCF import] improve MuJoCo importer, support collision filters
fixed a few more warnings

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<btCollisionShape*>	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;
 	}

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;
 }
 	

examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp

@@ -51,6 +51,8 @@ static btAlignedObjectArray<std::string> 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(

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);
     }
     

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);

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<std::string*> m_strings;
 
 	btAlignedObjectArray<btCollisionShape*>	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;
 

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_projectionMatrix,
-                                                                                 m_multiThreadedHelper->m_pixelsRGBA,
+				 m_multiThreadedHelper->m_pixelsRGBA,
-                                                                                 m_multiThreadedHelper->m_rgbaBufferSizeInPixels,
+				 m_multiThreadedHelper->m_rgbaBufferSizeInPixels,
-                                                                                 m_multiThreadedHelper->m_depthBuffer,
+				 m_multiThreadedHelper->m_depthBuffer,
-                                                                                 m_multiThreadedHelper->m_depthBufferSizeInPixels,
+				 m_multiThreadedHelper->m_depthBufferSizeInPixels,
-                                                                                 m_multiThreadedHelper->m_segmentationMaskBuffer,
+				 m_multiThreadedHelper->m_segmentationMaskBuffer,
-                                                                                 m_multiThreadedHelper->m_segmentationMaskBufferSizeInPixels,
+				 m_multiThreadedHelper->m_segmentationMaskBufferSizeInPixels,
-                                                                                 m_multiThreadedHelper->m_startPixelIndex, 
+				 m_multiThreadedHelper->m_startPixelIndex, 
-                                                                                 m_multiThreadedHelper->m_destinationWidth, 
+				 m_multiThreadedHelper->m_destinationWidth, 
-                                                                                 m_multiThreadedHelper->m_destinationHeight, 
+				 m_multiThreadedHelper->m_destinationHeight, 
-                                                                                 m_multiThreadedHelper->m_numPixelsCopied);
+				 m_multiThreadedHelper->m_numPixelsCopied);
  		m_multiThreadedHelper->mainThreadRelease();
             break;
         }

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();