Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add getClosestPoints.py example.

Browse Source 
allow to perform a getClosestPoints query with a collisionShape and world transform (position, orientation) that isn't part of the world.
(use createCollisionShape to create it)
This commit is contained in:
erwincoumans
2018-09-22 13:17:09 -07:00
parent 64a7a3d82f
commit cdf8c908ad
6 changed files with 270 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -3883,10 +3883,76 @@ B3_SHARED_API void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle com
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags += CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD;
command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD;
command->m_requestContactPointArguments.m_closestDistanceThreshold = distance;
}
B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeA(b3SharedMemoryCommandHandle commandHandle, int collisionShapeA)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_A;
command->m_requestContactPointArguments.m_collisionShapeA = collisionShapeA;
}
B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeB(b3SharedMemoryCommandHandle commandHandle, int collisionShapeB)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_B;
command->m_requestContactPointArguments.m_collisionShapeB = collisionShapeB;
}
B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapePositionA(b3SharedMemoryCommandHandle commandHandle, double collisionShapePositionA[/*3*/])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_A;
command->m_requestContactPointArguments.m_collisionShapePositionA[0] = collisionShapePositionA[0];
command->m_requestContactPointArguments.m_collisionShapePositionA[1] = collisionShapePositionA[1];
command->m_requestContactPointArguments.m_collisionShapePositionA[2] = collisionShapePositionA[2];
}
B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapePositionB(b3SharedMemoryCommandHandle commandHandle, double collisionShapePositionB[/*3*/])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_B;
command->m_requestContactPointArguments.m_collisionShapePositionB[0] = collisionShapePositionB[0];
command->m_requestContactPointArguments.m_collisionShapePositionB[1] = collisionShapePositionB[1];
command->m_requestContactPointArguments.m_collisionShapePositionB[2] = collisionShapePositionB[2];
}
B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeOrientationA(b3SharedMemoryCommandHandle commandHandle, double collisionShapeOrientationA[/*4*/])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_A;
command->m_requestContactPointArguments.m_collisionShapeOrientationA[0] = collisionShapeOrientationA[0];
command->m_requestContactPointArguments.m_collisionShapeOrientationA[1] = collisionShapeOrientationA[1];
command->m_requestContactPointArguments.m_collisionShapeOrientationA[2] = collisionShapeOrientationA[2];
command->m_requestContactPointArguments.m_collisionShapeOrientationA[3] = collisionShapeOrientationA[3];
}
B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeOrientationB(b3SharedMemoryCommandHandle commandHandle, double collisionShapeOrientationB[/*4*/])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_B;
command->m_requestContactPointArguments.m_collisionShapeOrientationB[0] = collisionShapeOrientationB[0];
command->m_requestContactPointArguments.m_collisionShapeOrientationB[1] = collisionShapeOrientationB[1];
command->m_requestContactPointArguments.m_collisionShapeOrientationB[2] = collisionShapeOrientationB[2];
command->m_requestContactPointArguments.m_collisionShapeOrientationB[3] = collisionShapeOrientationB[3];
}
///get all the bodies that touch a given axis aligned bounding box specified in world space (min and max coordinates)
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[3], const double aabbMax[3])

9
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -290,6 +290,15 @@ B3_SHARED_API void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle c
B3_SHARED_API void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
B3_SHARED_API void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
B3_SHARED_API void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance);
B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeA(b3SharedMemoryCommandHandle commandHandle, int collisionShapeA);
B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeB(b3SharedMemoryCommandHandle commandHandle, int collisionShapeB);
B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapePositionA(b3SharedMemoryCommandHandle commandHandle, double collisionShapePositionA[/*3*/]);
B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapePositionB(b3SharedMemoryCommandHandle commandHandle, double collisionShapePositionB[/*3*/]);
B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeOrientationA(b3SharedMemoryCommandHandle commandHandle, double collisionShapeOrientationA[/*4*/]);
B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeOrientationB(b3SharedMemoryCommandHandle commandHandle, double collisionShapeOrientationB[/*4*/]);
B3_SHARED_API void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
///get all the bodies that touch a given axis aligned bounding box specified in world space (min and max coordinates)

90
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1693,6 +1693,7 @@ struct PhysicsServerCommandProcessorInternalData
b3HashMap<b3HashString, UrdfVisualShapeCache> m_cachedVUrdfisualShapes;
b3ThreadPool* m_threadPool;
btScalar m_defaultCollisionMargin;
PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc)
:m_pluginManager(proc),
@@ -1725,7 +1726,8 @@ struct PhysicsServerCommandProcessorInternalData
m_pdControlPlugin(-1),
m_collisionFilterPlugin(-1),
m_grpcPlugin(-1),
m_threadPool(0)
m_threadPool(0),
m_defaultCollisionMargin(0.001)
{
{
@@ -3965,7 +3967,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
{
bool hasStatus = true;
serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED;
btScalar defaultCollisionMargin = 0.001;
btMultiBodyWorldImporter* worldImporter = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
@@ -3977,6 +3978,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
if (clientCmd.m_createUserShapeArgs.m_numUserShapes>1)
{
compound = worldImporter->createCompoundShape();
compound ->setMargin(m_data->m_defaultCollisionMargin);
}
for (int i = 0; i < clientCmd.m_createUserShapeArgs.m_numUserShapes; i++)
{
@@ -4015,6 +4017,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
{
double radius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
shape = worldImporter->createSphereShape(radius);
shape->setMargin(m_data->m_defaultCollisionMargin);
if (compound)
{
compound->addChildShape(childTransform, shape);
@@ -4031,6 +4034,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[2]);
shape = worldImporter->createBoxShape(halfExtents);
shape->setMargin(m_data->m_defaultCollisionMargin);
if (compound)
{
compound->addChildShape(childTransform, shape);
@@ -4043,6 +4047,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
{
shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
shape->setMargin(m_data->m_defaultCollisionMargin);
if (compound)
{
compound->addChildShape(childTransform, shape);
@@ -4057,6 +4062,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
{
shape = worldImporter->createCylinderShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
0.5*clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
shape->setMargin(m_data->m_defaultCollisionMargin);
if (compound)
{
compound->addChildShape(childTransform, shape);
@@ -4074,6 +4080,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]);
shape = worldImporter->createPlaneShape(planeNormal, 0);
shape->setMargin(m_data->m_defaultCollisionMargin);
if (compound)
{
compound->addChildShape(childTransform, shape);
@@ -4145,12 +4152,12 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
{
compound = worldImporter->createCompoundShape();
}
compound->setMargin(defaultCollisionMargin);
compound->setMargin(m_data->m_defaultCollisionMargin);
for (int s = 0; s < (int)shapes.size(); s++)
{
btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
convexHull->setMargin(defaultCollisionMargin);
convexHull->setMargin(m_data->m_defaultCollisionMargin);
tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size();
@@ -4253,6 +4260,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
if (compound)
{
compound->addChildShape(childTransform, shape);
shape->setMargin(m_data->m_defaultCollisionMargin);
}
}
delete glmesh;
@@ -4265,11 +4273,11 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
{
compound = worldImporter->createCompoundShape();
}
compound->setMargin(defaultCollisionMargin);
compound->setMargin(m_data->m_defaultCollisionMargin);
{
btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
convexHull->setMargin(defaultCollisionMargin);
convexHull->setMargin(m_data->m_defaultCollisionMargin);
for (int v = 0; v < convertedVerts.size(); v++)
{
@@ -6236,6 +6244,76 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand
btAlignedObjectArray<int> setALinkIndex;
btAlignedObjectArray<int> setBLinkIndex;
btCollisionObject colObA;
btCollisionObject colObB;
int collisionShapeA = (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_A) ?
clientCmd.m_requestContactPointArguments.m_collisionShapeA : -1;
int collisionShapeB = (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_B) ?
clientCmd.m_requestContactPointArguments.m_collisionShapeB : -1;
if (collisionShapeA>=0)
{
btVector3 posA(0,0,0);
if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_A)
{
posA.setValue(clientCmd.m_requestContactPointArguments.m_collisionShapePositionA[0],
clientCmd.m_requestContactPointArguments.m_collisionShapePositionA[1],
clientCmd.m_requestContactPointArguments.m_collisionShapePositionA[2]);
}
btQuaternion ornA(0,0,0,1);
if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_A)
{
ornA.setValue(clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[0],
clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[1],
clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[2],
clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[3]);
}
InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeA);
if (handle && handle->m_collisionShape)
{
colObA.setCollisionShape(handle->m_collisionShape);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posA);
tr.setRotation(ornA);
colObA.setWorldTransform(tr);
setA.push_back(&colObA);
setALinkIndex.push_back(-2);
}
}
if (collisionShapeB>=0)
{
btVector3 posB(0,0,0);
if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_B)
{
posB.setValue(clientCmd.m_requestContactPointArguments.m_collisionShapePositionB[0],
clientCmd.m_requestContactPointArguments.m_collisionShapePositionB[1],
clientCmd.m_requestContactPointArguments.m_collisionShapePositionB[2]);
}
btQuaternion ornB(0,0,0,1);
if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_B)
{
ornB.setValue(clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationB[0],
clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationB[1],
clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationB[2],
clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationB[3]);
}
InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeB);
if (handle && handle->m_collisionShape)
{
colObB.setCollisionShape(handle->m_collisionShape);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posB);
tr.setRotation(ornB);
colObB.setWorldTransform(tr);
setB.push_back(&colObB);
setBLinkIndex.push_back(-2);
}
}
if (bodyUniqueIdA >= 0)
{
InternalBodyData* bodyA = m_data->m_bodyHandles.getHandle(bodyUniqueIdA);

16
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -278,6 +278,14 @@ enum EnumRequestContactDataUpdateFlags
CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD=2,
CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER = 4,
CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER = 8,
CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_A = 16,
CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_B = 32,
CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_A = 64,
CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_B = 128,
CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_A= 256,
CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_B= 512,
};
struct RequestRaycastIntersections
@@ -309,6 +317,14 @@ struct RequestContactDataArgs
int m_linkIndexAIndexFilter;
int m_linkIndexBIndexFilter;
double m_closestDistanceThreshold;
int m_collisionShapeA;
int m_collisionShapeB;
double m_collisionShapePositionA[3];
double m_collisionShapePositionB[3];
double m_collisionShapeOrientationA[4];
double m_collisionShapeOrientationB[4];
int m_mode;
};

38
examples/pybullet/examples/getClosestPoints.py Normal file
View File

@@ -0,0 +1,38 @@
import pybullet as p
import time
p.connect(p.GUI)
useCollisionShapeQuery = False
geom = p.createCollisionShape(p.GEOM_SPHERE, radius=0.1)
geomBox = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2,0.2,0.2])
obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom,basePosition=[0.5,0,1])
baseOrientationB = p.getQuaternionFromEuler([0,0.3,0])#[0,0.5,0.5,0]
basePositionB = [1.5,0,1]
obB = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geomBox,basePosition=basePositionB,baseOrientation=baseOrientationB )
lineWidth=3
colorRGB=[1,0,0]
lineId=p.addUserDebugLine(lineFromXYZ=[0,0,0],lineToXYZ=[0,0,0],lineColorRGB=colorRGB,lineWidth=lineWidth,lifeTime=0)
pitch=0
while (p.isConnected()):
pitch += 0.01
if (pitch>=3.1415*2.):
pitch=0
baseOrientationB = p.getQuaternionFromEuler([0,pitch,0])#[0,0.5,0.5,0]
p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB)
if (useCollisionShapeQuery):
pts = p.getClosestPoints(bodyA=-1, bodyB=-1, distance=100, collisionShapeA=geom,collisionShapeB=geomBox, collisionShapePositionA=[0.5,0,1],collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB)
else:
pts = p.getClosestPoints(bodyA=obA, bodyB=obB, distance=100)
if len(pts)>0:
#print(pts)
distance = pts[0][8]
#print("distance=",distance)
ptA = pts[0][5]
ptB = pts[0][6]
p.addUserDebugLine(lineFromXYZ=ptA,lineToXYZ=ptB,lineColorRGB=colorRGB,lineWidth=lineWidth,lifeTime=0,replaceItemUniqueId=lineId);
time.sleep(1./240.)

55
examples/pybullet/pybullet.c
View File

@@ -6002,6 +6002,17 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
int bodyUniqueIdB = -1;
int linkIndexA = -2;
int linkIndexB = -2;
int collisionShapeA = -1;
int collisionShapeB = -1;
PyObject* collisionShapePositionAObj = 0;
PyObject* collisionShapeOrientationAObj = 0;
double collisionShapePositionA[3]={0,0,0};
double collisionShapeOrientationA[4]={0,0,0,1};
PyObject* collisionShapePositionBObj = 0;
PyObject* collisionShapeOrientationBObj = 0;
double collisionShapePositionB[3]={0,0,0};
double collisionShapeOrientationB[4]={0,0,0,1};
double distanceThreshold = 0.f;
@@ -6011,10 +6022,14 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
int statusType;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"bodyA", "bodyB", "distance", "linkIndexA", "linkIndexB", "physicsClientId", NULL};
static char* kwlist[] = {"bodyA", "bodyB", "distance", "linkIndexA", "linkIndexB", "collisionShapeA","collisionShapeB", "collisionShapePositionA","collisionShapePositionB","collisionShapeOrientationA","collisionShapeOrientationB","physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|iii", kwlist,
&bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|iiiiOOOOi", kwlist,
&bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB,
&collisionShapeA, &collisionShapeB,
&collisionShapePositionAObj,&collisionShapePositionBObj,
&collisionShapeOrientationA,&collisionShapeOrientationBObj,
&physicsClientId))
return NULL;
sm = getPhysicsClient(physicsClientId);
@@ -6025,8 +6040,14 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
}
commandHandle = b3InitClosestDistanceQuery(sm);
if (bodyUniqueIdA>=0)
{
b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA);
}
if (bodyUniqueIdB>=0)
{
b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB);
}
b3SetClosestDistanceThreshold(commandHandle, distanceThreshold);
if (linkIndexA >= -1)
{
@@ -6036,6 +6057,34 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
{
b3SetClosestDistanceFilterLinkB(commandHandle, linkIndexB);
}
if (collisionShapeA>=0)
{
b3SetClosestDistanceFilterCollisionShapeA(commandHandle, collisionShapeA);
}
if (collisionShapeB>=0)
{
b3SetClosestDistanceFilterCollisionShapeB(commandHandle, collisionShapeB);
}
if (collisionShapePositionAObj)
{
pybullet_internalSetVectord(collisionShapePositionAObj,collisionShapePositionA);
b3SetClosestDistanceFilterCollisionShapePositionA(commandHandle, collisionShapePositionA);
}
if (collisionShapePositionBObj)
{
pybullet_internalSetVectord(collisionShapePositionBObj,collisionShapePositionB);
b3SetClosestDistanceFilterCollisionShapePositionB(commandHandle, collisionShapePositionB);
}
if (collisionShapeOrientationAObj)
{
pybullet_internalSetVector4d(collisionShapeOrientationAObj,collisionShapeOrientationA);
b3SetClosestDistanceFilterCollisionShapeOrientationA(commandHandle, collisionShapeOrientationA);
}
if (collisionShapeOrientationBObj)
{
pybullet_internalSetVector4d(collisionShapeOrientationBObj,collisionShapeOrientationB);
b3SetClosestDistanceFilterCollisionShapeOrientationB(commandHandle, collisionShapeOrientationB);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);