Implement rayTestBatch. At the moment, it is still testing individual rays on the physics server. We can enable multi-threaded version later. At least the python + shared-memory IPC overhead will be much lower.

This commit is contained in:
Erwin Coumans
2017-04-05 15:21:26 -07:00
parent e32debdca4
commit 6cbb00fd6b
6 changed files with 274 additions and 60 deletions

View File

@@ -1383,17 +1383,52 @@ b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle phy
struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand(); struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
b3Assert(command); b3Assert(command);
command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS; command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
command->m_requestRaycastIntersections.m_rayFromPosition[0] = rayFromWorldX; command->m_requestRaycastIntersections.m_numRays = 1;
command->m_requestRaycastIntersections.m_rayFromPosition[1] = rayFromWorldY; command->m_requestRaycastIntersections.m_rayFromPositions[0][0] = rayFromWorldX;
command->m_requestRaycastIntersections.m_rayFromPosition[2] = rayFromWorldZ; command->m_requestRaycastIntersections.m_rayFromPositions[0][1] = rayFromWorldY;
command->m_requestRaycastIntersections.m_rayToPosition[0] = rayToWorldX; command->m_requestRaycastIntersections.m_rayFromPositions[0][2] = rayFromWorldZ;
command->m_requestRaycastIntersections.m_rayToPosition[1] = rayToWorldY; command->m_requestRaycastIntersections.m_rayToPositions[0][0] = rayToWorldX;
command->m_requestRaycastIntersections.m_rayToPosition[2] = rayToWorldZ; command->m_requestRaycastIntersections.m_rayToPositions[0][1] = rayToWorldY;
command->m_requestRaycastIntersections.m_rayToPositions[0][2] = rayToWorldZ;
return (b3SharedMemoryCommandHandle)command; return (b3SharedMemoryCommandHandle)command;
} }
b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient *cl = (PhysicsClient *)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
command->m_updateFlags = 0;
command->m_requestRaycastIntersections.m_numRays = 0;
return (b3SharedMemoryCommandHandle)command;
}
void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
if (command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS)
{
int numRays = command->m_requestRaycastIntersections.m_numRays;
if (numRays<MAX_RAY_INTERSECTION_BATCH_SIZE)
{
command->m_requestRaycastIntersections.m_rayFromPositions[numRays][0] = rayFromWorld[0];
command->m_requestRaycastIntersections.m_rayFromPositions[numRays][1] = rayFromWorld[1];
command->m_requestRaycastIntersections.m_rayFromPositions[numRays][2] = rayFromWorld[2];
command->m_requestRaycastIntersections.m_rayToPositions[numRays][0] = rayToWorld[0];
command->m_requestRaycastIntersections.m_rayToPositions[numRays][1] = rayToWorld[1];
command->m_requestRaycastIntersections.m_rayToPositions[numRays][2] = rayToWorld[2];
command->m_requestRaycastIntersections.m_numRays++;
}
}
}
void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo) void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo)
{ {
PhysicsClient* cl = (PhysicsClient* ) physClient; PhysicsClient* cl = (PhysicsClient* ) physClient;

View File

@@ -332,6 +332,9 @@ b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle phy
double rayFromWorldY, double rayFromWorldZ, double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ); double rayToWorldX, double rayToWorldY, double rayToWorldZ);
b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient);
void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]);
void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo); void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo);

View File

@@ -2235,58 +2235,74 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_REQUEST_RAY_CAST_INTERSECTIONS: case CMD_REQUEST_RAY_CAST_INTERSECTIONS:
{ {
BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS"); BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS");
btVector3 rayFromWorld(clientCmd.m_requestRaycastIntersections.m_rayFromPosition[0],
clientCmd.m_requestRaycastIntersections.m_rayFromPosition[1],
clientCmd.m_requestRaycastIntersections.m_rayFromPosition[2]);
btVector3 rayToWorld(clientCmd.m_requestRaycastIntersections.m_rayToPosition[0],
clientCmd.m_requestRaycastIntersections.m_rayToPosition[1],
clientCmd.m_requestRaycastIntersections.m_rayToPosition[2]);
btCollisionWorld::ClosestRayResultCallback rayResultCallback(rayFromWorld,rayToWorld);
m_data->m_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,rayResultCallback);
serverStatusOut.m_raycastHits.m_numRaycastHits = 0; serverStatusOut.m_raycastHits.m_numRaycastHits = 0;
if (rayResultCallback.hasHit()) for (int ray=0;ray<clientCmd.m_requestRaycastIntersections.m_numRays;ray++)
{ {
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitFraction btVector3 rayFromWorld(clientCmd.m_requestRaycastIntersections.m_rayFromPositions[ray][0],
= rayResultCallback.m_closestHitFraction; clientCmd.m_requestRaycastIntersections.m_rayFromPositions[ray][1],
clientCmd.m_requestRaycastIntersections.m_rayFromPositions[ray][2]);
btVector3 rayToWorld(clientCmd.m_requestRaycastIntersections.m_rayToPositions[ray][0],
clientCmd.m_requestRaycastIntersections.m_rayToPositions[ray][1],
clientCmd.m_requestRaycastIntersections.m_rayToPositions[ray][2]);
int objectUniqueId = -1; btCollisionWorld::ClosestRayResultCallback rayResultCallback(rayFromWorld,rayToWorld);
int linkIndex = -1; m_data->m_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,rayResultCallback);
int rayHits = serverStatusOut.m_raycastHits.m_numRaycastHits;
const btRigidBody* body = btRigidBody::upcast(rayResultCallback.m_collisionObject); if (rayResultCallback.hasHit())
if (body)
{ {
objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2(); serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitFraction
= rayResultCallback.m_closestHitFraction;
int objectUniqueId = -1;
int linkIndex = -1;
const btRigidBody* body = btRigidBody::upcast(rayResultCallback.m_collisionObject);
if (body)
{
objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2();
} else
{
const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(rayResultCallback.m_collisionObject);
if (mblB && mblB->m_multiBody)
{
linkIndex = mblB->m_link;
objectUniqueId = mblB->m_multiBody->getUserIndex2();
}
}
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitObjectUniqueId
= objectUniqueId;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitObjectLinkIndex
= linkIndex;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[0]
= rayResultCallback.m_hitPointWorld[0];
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[1]
= rayResultCallback.m_hitPointWorld[1];
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[2]
= rayResultCallback.m_hitPointWorld[2];
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[0]
= rayResultCallback.m_hitNormalWorld[0];
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[1]
= rayResultCallback.m_hitNormalWorld[1];
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[2]
= rayResultCallback.m_hitNormalWorld[2];
} else } else
{ {
const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(rayResultCallback.m_collisionObject); serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitFraction = 1;
if (mblB && mblB->m_multiBody) serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectUniqueId = -1;
{ serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectLinkIndex = -1;
linkIndex = mblB->m_link; serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[0] = 0;
objectUniqueId = mblB->m_multiBody->getUserIndex2(); serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[1] = 0;
} serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[2] = 0;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[0] = 0;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[1] = 0;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[2] = 0;
} }
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectUniqueId
= objectUniqueId;
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectLinkIndex
= linkIndex;
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[0]
= rayResultCallback.m_hitPointWorld[0];
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[1]
= rayResultCallback.m_hitPointWorld[1];
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[2]
= rayResultCallback.m_hitPointWorld[2];
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[0]
= rayResultCallback.m_hitNormalWorld[0];
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[1]
= rayResultCallback.m_hitNormalWorld[1];
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[2]
= rayResultCallback.m_hitNormalWorld[2];
serverStatusOut.m_raycastHits.m_numRaycastHits++; serverStatusOut.m_raycastHits.m_numRaycastHits++;
} }
serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED; serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED;

View File

@@ -185,14 +185,15 @@ enum EnumRequestContactDataUpdateFlags
struct RequestRaycastIntersections struct RequestRaycastIntersections
{ {
double m_rayFromPosition[3]; int m_numRays;
double m_rayToPosition[3]; double m_rayFromPositions[MAX_RAY_INTERSECTION_BATCH_SIZE][3];
double m_rayToPositions[MAX_RAY_INTERSECTION_BATCH_SIZE][3];
}; };
struct SendRaycastHits struct SendRaycastHits
{ {
int m_numRaycastHits; int m_numRaycastHits;
b3RayHitInfo m_rayHits[MAX_RAY_HITS]; b3RayHitInfo m_rayHits[MAX_RAY_INTERSECTION_BATCH_SIZE];
}; };
struct RequestContactDataArgs struct RequestContactDataArgs

View File

@@ -257,9 +257,13 @@ enum b3VREventType
#define MAX_VR_BUTTONS 64 #define MAX_VR_BUTTONS 64
#define MAX_VR_CONTROLLERS 8 #define MAX_VR_CONTROLLERS 8
#define MAX_RAY_HITS 128
#define MAX_RAY_INTERSECTION_BATCH_SIZE 1024
#define MAX_RAY_HITS MAX_RAY_INTERSECTION_BATCH_SIZE
#define MAX_KEYBOARD_EVENTS 256 #define MAX_KEYBOARD_EVENTS 256
enum b3VRButtonInfo enum b3VRButtonInfo
{ {
eButtonIsDown = 1, eButtonIsDown = 1,

View File

@@ -400,8 +400,8 @@ static PyObject* pybullet_disconnectPhysicsServer(PyObject* self,
return Py_None; return Py_None;
} }
///to avoid memory leaks, disconnect all physics servers explicitly
void b3pybulletExitFunc() void b3pybulletExitFunc(void)
{ {
int i; int i;
for (i=0;i<MAX_PHYSICS_CLIENTS;i++) for (i=0;i<MAX_PHYSICS_CLIENTS;i++)
@@ -2791,7 +2791,7 @@ static PyObject* pybullet_setTimeOut(PyObject* self, PyObject* args, PyObject* k
return Py_None; return Py_None;
} }
static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject* keywds) static PyObject* pybullet_rayTestObsolete(PyObject* self, PyObject* args, PyObject* keywds)
{ {
b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
@@ -2875,6 +2875,154 @@ static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject* keyw
return Py_None; return Py_None;
} }
static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* keywds)
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
PyObject* rayFromObjList = 0;
PyObject* rayToObjList = 0;
b3PhysicsClientHandle sm = 0;
int sizeFrom = 0;
int sizeTo = 0;
static char* kwlist[] = {"rayFromPositions", "rayToPositions", "physicsClientId", NULL};
int physicsClientId = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist,
&rayFromObjList, &rayToObjList, &physicsClientId))
return NULL;
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
commandHandle = b3CreateRaycastBatchCommandInit(sm);
if (rayFromObjList)
{
PyObject* seqRayFromObj = PySequence_Fast(rayFromObjList, "expected a sequence of rayFrom positions");
PyObject* seqRayToObj = PySequence_Fast(rayToObjList, "expected a sequence of 'rayTo' positions");
if (seqRayFromObj && seqRayToObj)
{
int lenFrom = PySequence_Size(rayFromObjList);
int lenTo= PySequence_Size(seqRayToObj);
if (lenFrom!=lenTo)
{
PyErr_SetString(SpamError, "Size of from_positions need to be equal to size of to_positions.");
Py_DECREF(seqRayFromObj);
Py_DECREF(seqRayToObj);
return NULL;
} else
{
int i;
if (lenFrom >= MAX_RAY_INTERSECTION_BATCH_SIZE)
{
PyErr_SetString(SpamError, "Number of rays exceed the maximum batch size.");
Py_DECREF(seqRayFromObj);
Py_DECREF(seqRayToObj);
return NULL;
}
for (i = 0; i < lenFrom; i++)
{
PyObject* rayFromObj = PySequence_GetItem(rayFromObjList,i);
PyObject* rayToObj = PySequence_GetItem(seqRayToObj,i);
double rayFromWorld[3];
double rayToWorld[3];
if ((pybullet_internalSetVectord(rayFromObj, rayFromWorld)) &&
(pybullet_internalSetVectord(rayToObj, rayToWorld)))
{
b3RaycastBatchAddRay(commandHandle, rayFromWorld, rayToWorld);
} else
{
PyErr_SetString(SpamError, "Items in the from/to positions need to be an [x,y,z] list of 3 floats/doubles");
Py_DECREF(seqRayFromObj);
Py_DECREF(seqRayToObj);
Py_DECREF(rayFromObj);
Py_DECREF(rayToObj);
return NULL;
}
Py_DECREF(rayFromObj);
Py_DECREF(rayToObj);
}
}
} else
{
}
if (seqRayFromObj)
{
Py_DECREF(seqRayFromObj);
}
if (seqRayToObj)
{
Py_DECREF(seqRayToObj);
}
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED)
{
struct b3RaycastInformation raycastInfo;
PyObject* rayHitsObj = 0;
int i;
b3GetRaycastInformation(sm, &raycastInfo);
rayHitsObj = PyTuple_New(raycastInfo.m_numRayHits);
for (i = 0; i < raycastInfo.m_numRayHits; i++)
{
PyObject* singleHitObj = PyTuple_New(5);
{
PyObject* ob = PyInt_FromLong(raycastInfo.m_rayHits[i].m_hitObjectUniqueId);
PyTuple_SetItem(singleHitObj, 0, ob);
}
{
PyObject* ob = PyInt_FromLong(raycastInfo.m_rayHits[i].m_hitObjectLinkIndex);
PyTuple_SetItem(singleHitObj, 1, ob);
}
{
PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitFraction);
PyTuple_SetItem(singleHitObj, 2, ob);
}
{
PyObject* posObj = PyTuple_New(3);
int p;
for (p = 0; p < 3; p++)
{
PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitPositionWorld[p]);
PyTuple_SetItem(posObj, p, ob);
}
PyTuple_SetItem(singleHitObj, 3, posObj);
}
{
PyObject* normalObj = PyTuple_New(3);
int p;
for (p = 0; p < 3; p++)
{
PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitNormalWorld[p]);
PyTuple_SetItem(normalObj, p, ob);
}
PyTuple_SetItem(singleHitObj, 4, normalObj);
}
PyTuple_SetItem(rayHitsObj, i, singleHitObj);
}
return rayHitsObj;
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_getMatrixFromQuaternion(PyObject* self, PyObject* args) static PyObject* pybullet_getMatrixFromQuaternion(PyObject* self, PyObject* args)
{ {
PyObject* quatObj; PyObject* quatObj;
@@ -5298,9 +5446,14 @@ static PyMethodDef SpamMethods[] = {
"fileName, optional objectUniqueId. Function returns int loggingUniqueId"}, "fileName, optional objectUniqueId. Function returns int loggingUniqueId"},
{"stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS, {"stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS,
"Stop logging of robot state, given a loggingUniqueId."}, "Stop logging of robot state, given a loggingUniqueId."},
{"rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS, {"rayTest", (PyCFunction)pybullet_rayTestObsolete, METH_VARARGS | METH_KEYWORDS,
"Cast a ray and return the first object hit, if any. " "Cast a ray and return the first object hit, if any. "
"Takes two arguments (from position [x,y,z] and to position [x,y,z] in Cartesian world coordinates"}, "Takes two arguments (from_position [x,y,z] and to_position [x,y,z] in Cartesian world coordinates"},
{"rayTestBatch", (PyCFunction)pybullet_rayTestBatch, METH_VARARGS | METH_KEYWORDS,
"Cast a batch of rays and return the result for each of the rays (first object hit, if any. or -1) "
"Takes two arguments (list of from_positions [x,y,z] and a list of to_positions [x,y,z] in Cartesian world coordinates"},
{"setTimeOut", (PyCFunction)pybullet_setTimeOut, METH_VARARGS | METH_KEYWORDS, {"setTimeOut", (PyCFunction)pybullet_setTimeOut, METH_VARARGS | METH_KEYWORDS,
"Set the timeOut in seconds, used for most of the API calls."}, "Set the timeOut in seconds, used for most of the API calls."},
// todo(erwincoumans) // todo(erwincoumans)
@@ -5436,7 +5589,9 @@ initpybullet(void)
PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE); PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE);
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION);
PyModule_AddIntConstant(m, "MAX_RAY_INTERSECTION_BATCH_SIZE", MAX_RAY_INTERSECTION_BATCH_SIZE);
PyModule_AddIntConstant(m, "B3G_F1", B3G_F1); PyModule_AddIntConstant(m, "B3G_F1", B3G_F1);
PyModule_AddIntConstant(m, "B3G_F2", B3G_F2); PyModule_AddIntConstant(m, "B3G_F2", B3G_F2);
PyModule_AddIntConstant(m, "B3G_F3", B3G_F3); PyModule_AddIntConstant(m, "B3G_F3", B3G_F3);