From 6cbb00fd6b962baaeb044ccd688894ce742feca1 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 5 Apr 2017 15:21:26 -0700 Subject: [PATCH] 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. --- examples/SharedMemory/PhysicsClientC_API.cpp | 49 ++++- examples/SharedMemory/PhysicsClientC_API.h | 3 + .../PhysicsServerCommandProcessor.cpp | 102 ++++++----- examples/SharedMemory/SharedMemoryCommands.h | 7 +- examples/SharedMemory/SharedMemoryPublic.h | 6 +- examples/pybullet/pybullet.c | 167 +++++++++++++++++- 6 files changed, 274 insertions(+), 60 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index b0695cd20..c332e34a7 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1383,17 +1383,52 @@ b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle phy struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand(); b3Assert(command); command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS; - command->m_requestRaycastIntersections.m_rayFromPosition[0] = rayFromWorldX; - command->m_requestRaycastIntersections.m_rayFromPosition[1] = rayFromWorldY; - command->m_requestRaycastIntersections.m_rayFromPosition[2] = rayFromWorldZ; - command->m_requestRaycastIntersections.m_rayToPosition[0] = rayToWorldX; - command->m_requestRaycastIntersections.m_rayToPosition[1] = rayToWorldY; - command->m_requestRaycastIntersections.m_rayToPosition[2] = rayToWorldZ; + command->m_requestRaycastIntersections.m_numRays = 1; + command->m_requestRaycastIntersections.m_rayFromPositions[0][0] = rayFromWorldX; + command->m_requestRaycastIntersections.m_rayFromPositions[0][1] = rayFromWorldY; + command->m_requestRaycastIntersections.m_rayFromPositions[0][2] = rayFromWorldZ; + command->m_requestRaycastIntersections.m_rayToPositions[0][0] = rayToWorldX; + command->m_requestRaycastIntersections.m_rayToPositions[0][1] = rayToWorldY; + command->m_requestRaycastIntersections.m_rayToPositions[0][2] = rayToWorldZ; 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 (numRaysm_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) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 35071ae34..0f65a8fa6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -332,6 +332,9 @@ b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle phy double rayFromWorldY, double rayFromWorldZ, 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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 46d5ebabd..b8dbcde84 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2235,58 +2235,74 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm case 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; - if (rayResultCallback.hasHit()) + for (int ray=0;raym_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,rayResultCallback); + int rayHits = serverStatusOut.m_raycastHits.m_numRaycastHits; - const btRigidBody* body = btRigidBody::upcast(rayResultCallback.m_collisionObject); - if (body) + if (rayResultCallback.hasHit()) { - 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 { - 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_hitFraction = 1; + 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; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[0] = 0; + 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_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index f5038c6a4..b7a3b1b7b 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -185,14 +185,15 @@ enum EnumRequestContactDataUpdateFlags struct RequestRaycastIntersections { - double m_rayFromPosition[3]; - double m_rayToPosition[3]; + int m_numRays; + double m_rayFromPositions[MAX_RAY_INTERSECTION_BATCH_SIZE][3]; + double m_rayToPositions[MAX_RAY_INTERSECTION_BATCH_SIZE][3]; }; struct SendRaycastHits { int m_numRaycastHits; - b3RayHitInfo m_rayHits[MAX_RAY_HITS]; + b3RayHitInfo m_rayHits[MAX_RAY_INTERSECTION_BATCH_SIZE]; }; struct RequestContactDataArgs diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index dacc6426e..b4f4976e2 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -257,9 +257,13 @@ enum b3VREventType #define MAX_VR_BUTTONS 64 #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 + + enum b3VRButtonInfo { eButtonIsDown = 1, diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 0e920f7bd..77d18c402 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -400,8 +400,8 @@ static PyObject* pybullet_disconnectPhysicsServer(PyObject* self, return Py_None; } - -void b3pybulletExitFunc() +///to avoid memory leaks, disconnect all physics servers explicitly +void b3pybulletExitFunc(void) { int i; for (i=0;i= 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) { PyObject* quatObj; @@ -5298,9 +5446,14 @@ static PyMethodDef SpamMethods[] = { "fileName, optional objectUniqueId. Function returns int loggingUniqueId"}, {"stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS, "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. " - "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, "Set the timeOut in seconds, used for most of the API calls."}, // 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_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_F2", B3G_F2); PyModule_AddIntConstant(m, "B3G_F3", B3G_F3);