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

59
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);
b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA);
b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB);
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);