mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-04 17:40:06 +00:00
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 <collision group="1" mask="2"/>. 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.
This commit is contained in:
erwincoumans
parent
bf3399d0e3
commit
9553892770
29
data/cube_collisionfilter.urdf
Normal file
29
data/cube_collisionfilter.urdf
Normal file
@ -0,0 +1,29 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot name="cube">
|
||||
<link name="baseLink">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="1.0"/>
|
||||
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="cube.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision group="0" mask="0">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="1 1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
|
||||
@ -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;v<link->m_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
|
||||
{
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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<int>(group_char);
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
const char* mask_char = config->Attribute("mask");
|
||||
if (mask_char)
|
||||
{
|
||||
collision.m_flags |= URDF_HAS_COLLISION_MASK;
|
||||
collision.m_collisionMask = urdfLexicalCast<int>(mask_char);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
const char *name_char = config->Attribute("name");
|
||||
if (name_char)
|
||||
|
||||
@ -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)
|
||||
{
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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<InternalBodyData*> 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;i<bodies.size();i++)
|
||||
{
|
||||
InternalBodyData* body = bodies[i];
|
||||
if (body)
|
||||
{
|
||||
if (body->m_multiBody)
|
||||
{
|
||||
|
||||
if (body->m_multiBody->getBaseCollider())
|
||||
{
|
||||
m_data->m_dynamicsWorld->refreshBroadphaseProxy(body->m_multiBody->getBaseCollider());
|
||||
}
|
||||
for (int i=0;i<body->m_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;
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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;
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -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<b3Notification> 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;
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
33
examples/SharedMemory/plugins/b3PluginCollisionInterface.h
Normal file
33
examples/SharedMemory/plugins/b3PluginCollisionInterface.h
Normal file
@ -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
|
||||
@ -7,11 +7,148 @@
|
||||
#include "../../SharedMemoryPublic.h"
|
||||
#include "../b3PluginContext.h"
|
||||
#include <stdio.h>
|
||||
#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<b3CustomCollisionFilter, b3CustomCollisionFilter> 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;i<arguments->m_numInts;i++)
|
||||
{
|
||||
printf("%d", arguments->m_ints[i]);
|
||||
if ((i+1)<arguments->m_numInts)
|
||||
{
|
||||
printf(",");
|
||||
}
|
||||
}
|
||||
printf("]\nfloat args: [");
|
||||
for (int i=0;i<arguments->m_numFloats;i++)
|
||||
{
|
||||
printf("%f", arguments->m_floats[i]);
|
||||
if ((i+1)<arguments->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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -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
|
||||
};
|
||||
|
||||
17
examples/pybullet/examples/collisionFilter.py
Normal file
17
examples/pybullet/examples/collisionFilter.py
Normal file
@ -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)
|
||||
@ -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."},
|
||||
|
||||
@ -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)
|
||||
{
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user