AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 21:30:09 +00:00
Code Issues Releases Wiki Activity

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

Browse Source 
[MJCF import] improve MuJoCo importer, support collision filters
fixed a few more warnings
This commit is contained in:
Erwin Coumans 2017-01-16 08:23:49 -08:00
parent c0c4c8ba3f
commit 8e9181f85c
8 changed files with 156 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
examples/CommonInterfaces/CommonMultiBodyBase.h
View File

@ -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;
@ -143,6 +189,12 @@ 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;
}

4
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
View File

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

10
examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
View File

@ -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(

6
examples/OpenGLWindow/SimpleOpenGL3App.cpp
View File

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

6
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

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

66
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

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

25
examples/SharedMemory/PhysicsServerExample.cpp
View File

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

5
src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
View File

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