diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index ab28ab411..35f487d7b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2736,13 +2736,19 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsCl struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand(); b3Assert(command); command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS; - command->m_requestRaycastIntersections.m_numRays = 0; + command->m_requestRaycastIntersections.m_numCommandRays = 1; + command->m_requestRaycastIntersections.m_numStreamingRays = 0; command->m_requestRaycastIntersections.m_numThreads = 1; - double rayFrom[3] = {rayFromWorldX,rayFromWorldY,rayFromWorldZ}; - double rayTo[3] = {rayToWorldX,rayToWorldY,rayToWorldZ}; - cl->uploadRaysToSharedMemory(*command, rayFrom, rayTo, 1); - + command->m_requestRaycastIntersections.m_numCommandRays = 1; + command->m_requestRaycastIntersections.m_fromToRays[0].m_rayFromPosition[0] = rayFromWorldX; + command->m_requestRaycastIntersections.m_fromToRays[0].m_rayFromPosition[1] = rayFromWorldY; + command->m_requestRaycastIntersections.m_fromToRays[0].m_rayFromPosition[2] = rayFromWorldZ; + + command->m_requestRaycastIntersections.m_fromToRays[0].m_rayToPosition[0] = rayToWorldX; + command->m_requestRaycastIntersections.m_fromToRays[0].m_rayToPosition[1] = rayToWorldY; + command->m_requestRaycastIntersections.m_fromToRays[0].m_rayToPosition[2] = rayToWorldZ; + return (b3SharedMemoryCommandHandle)command; } @@ -2755,7 +2761,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3Phys b3Assert(command); command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS; command->m_updateFlags = 0; - command->m_requestRaycastIntersections.m_numRays = 0; + command->m_requestRaycastIntersections.m_numCommandRays = 0; + command->m_requestRaycastIntersections.m_numStreamingRays = 0; command->m_requestRaycastIntersections.m_numThreads = 1; return (b3SharedMemoryCommandHandle)command; } @@ -2767,18 +2774,26 @@ B3_SHARED_API void b3RaycastBatchSetNumThreads(b3SharedMemoryCommandHandle comm command->m_requestRaycastIntersections.m_numThreads = numThreads; } -B3_SHARED_API void b3RaycastBatchAddRay(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]) +B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]) { - PhysicsClient* cl = (PhysicsClient* ) physClient; - b3Assert(cl); - 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) { - cl->uploadRaysToSharedMemory(*command, rayFromWorld, rayToWorld, 1); + int numRays = command->m_requestRaycastIntersections.m_numCommandRays; + if (numRaysm_requestRaycastIntersections.m_fromToRays[numRays].m_rayFromPosition[0] = rayFromWorld[0]; + command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayFromPosition[1] = rayFromWorld[1]; + command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayFromPosition[2] = rayFromWorld[2]; + + command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayToPosition[0] = rayToWorld[0]; + command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayToPosition[1] = rayToWorld[1]; + command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayToPosition[2] = rayToWorld[2]; + + command->m_requestRaycastIntersections.m_numCommandRays++; + } } } @@ -2790,7 +2805,7 @@ B3_SHARED_API void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3Sha struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS); - b3Assert(numRaysm_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS) { cl->uploadRaysToSharedMemory(*command, rayFromWorldArray, rayToWorldArray, numRays); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 6146a452e..e57aaa510 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -544,7 +544,9 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsCl B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient); // Sets the number of threads to use to compute the ray intersections for the batch. Specify 0 to let Bullet decide, 1 (default) for single core execution, 2 or more to select the number of threads to use. B3_SHARED_API void b3RaycastBatchSetNumThreads(b3SharedMemoryCommandHandle commandHandle, int numThreads); -B3_SHARED_API void b3RaycastBatchAddRay(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]); +//max num rays for b3RaycastBatchAddRay is MAX_RAY_INTERSECTION_BATCH_SIZE +B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]); +//max num rays for b3RaycastBatchAddRays is MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING B3_SHARED_API void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double* rayFromWorld, const double* rayToWorld, int numRays); B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 131cc5d5f..1f3d24c35 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1719,22 +1719,22 @@ void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, void PhysicsClientSharedMemory::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) { - int curNumRays = command.m_requestRaycastIntersections.m_numRays; - int newNumRays = curNumRays + numRays; - btAssert(newNumRaysm_testBlock1->m_bulletStreamDataServerToClientRefactor; - rayDataStream[curNumRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0]; - rayDataStream[curNumRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1]; - rayDataStream[curNumRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2]; - rayDataStream[curNumRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0]; - rayDataStream[curNumRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1]; - rayDataStream[curNumRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2]; - command.m_requestRaycastIntersections.m_numRays++; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0]; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1]; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2]; + command.m_requestRaycastIntersections.m_numStreamingRays++; } } diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 24fb40a29..45f85d75c 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -1355,22 +1355,22 @@ void PhysicsDirect::uploadBulletFileToSharedMemory(const char* data, int len) void PhysicsDirect::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) { - int curNumRays = command.m_requestRaycastIntersections.m_numRays; - int newNumRays = curNumRays + numRays; - btAssert(newNumRaysm_bulletStreamDataServerToClient; - rayDataStream[curNumRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0]; - rayDataStream[curNumRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1]; - rayDataStream[curNumRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2]; - rayDataStream[curNumRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0]; - rayDataStream[curNumRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1]; - rayDataStream[curNumRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2]; - command.m_requestRaycastIntersections.m_numRays++; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0]; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1]; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2]; + command.m_requestRaycastIntersections.m_numStreamingRays++; } } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0f27342b8..82391b4c2 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4826,21 +4826,30 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS"); serverStatusOut.m_raycastHits.m_numRaycastHits = 0; - const int numRays = clientCmd.m_requestRaycastIntersections.m_numRays; + const int numCommandRays = clientCmd.m_requestRaycastIntersections.m_numCommandRays; + const int numStreamingRays = clientCmd.m_requestRaycastIntersections.m_numStreamingRays; const int numThreads = clientCmd.m_requestRaycastIntersections.m_numThreads; + int totalRays = numCommandRays+numStreamingRays; btAlignedObjectArray rays; - rays.resize(numRays); - memcpy(&rays[0],bufferServerToClient,numRays*sizeof(b3RayData)); + rays.resize(totalRays); + if (numCommandRays) + { + memcpy(&rays[0],&clientCmd.m_requestRaycastIntersections.m_fromToRays[0],numCommandRays*sizeof(b3RayData)); + } + if (numStreamingRays) + { + memcpy(&rays[numCommandRays],bufferServerToClient,numStreamingRays*sizeof(b3RayData)); + } - BatchRayCaster batchRayCaster(m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo *)bufferServerToClient, numRays); + BatchRayCaster batchRayCaster(m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo *)bufferServerToClient, totalRays); if (numThreads == 0) { // When 0 is specified, Bullet can decide how many threads to use. // About 16 rays per thread seems to work reasonably well. - batchRayCaster.castRays(numRays / 16); + batchRayCaster.castRays(totalRays / 16); } else if (numThreads == 1) { // Sequentially trace all rays: - for (int i = 0; i < numRays; i++) { + for (int i = 0; i < totalRays; i++) { batchRayCaster.processRay(i); } } else { @@ -4849,7 +4858,7 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co batchRayCaster.castRays(numThreads); } - serverStatusOut.m_raycastHits.m_numRaycastHits = numRays; + serverStatusOut.m_raycastHits.m_numRaycastHits = totalRays; serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED; return hasStatus; } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 6df03e6f4..b94ed8666 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -287,14 +287,18 @@ struct RequestRaycastIntersections // 1: Use a single thread (i.e. no multi-threading) // 2 or more: Number of threads to use. int m_numThreads; - int m_numRays; - // Actual ray data stored in m_bulletStreamDataServerToClientRefactor. + int m_numCommandRays; + //m_numCommandRays command rays are stored in m_fromToRays + b3RayData m_fromToRays[MAX_RAY_INTERSECTION_BATCH_SIZE]; + + int m_numStreamingRays; + //streaming ray data stored in shared memory streaming part. (size m_numStreamingRays ) }; struct SendRaycastHits { int m_numRaycastHits; - // Actual ray data stored in m_bulletStreamDataServerToClientRefactor. + // Actual ray result data stored in shared memory streaming part. }; struct RequestContactDataArgs diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index f6467fb38..3023bfaf3 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -582,7 +582,10 @@ typedef union { struct b3RayData a; struct b3RayHitInfo b; } RAY_DATA_UNION; -#define MAX_RAY_INTERSECTION_BATCH_SIZE 16*1024 + + +#define MAX_RAY_INTERSECTION_BATCH_SIZE 256 +#define MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING 16*1024 #define MAX_RAY_HITS MAX_RAY_INTERSECTION_BATCH_SIZE #define VISUAL_SHAPE_MAX_PATH_LEN 1024 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 72a730982..71813938f 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4730,7 +4730,7 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* { int i; - if (lenFrom > MAX_RAY_INTERSECTION_BATCH_SIZE) + if (lenFrom > MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING) { PyErr_SetString(SpamError, "Number of rays exceed the maximum batch size."); Py_DECREF(seqRayFromObj); @@ -4748,7 +4748,9 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* if ((pybullet_internalSetVectord(rayFromObj, rayFromWorld)) && (pybullet_internalSetVectord(rayToObj, rayToWorld))) { - b3RaycastBatchAddRay(sm, commandHandle, rayFromWorld, rayToWorld); + //todo: better to upload all rays at once + //b3RaycastBatchAddRay(commandHandle, rayFromWorld, rayToWorld); + b3RaycastBatchAddRays(sm, commandHandle, rayFromWorld, rayToWorld,1); } else { PyErr_SetString(SpamError, "Items in the from/to positions need to be an [x,y,z] list of 3 floats/doubles"); @@ -9647,7 +9649,7 @@ initpybullet(void) PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS", URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS); - PyModule_AddIntConstant(m, "MAX_RAY_INTERSECTION_BATCH_SIZE", MAX_RAY_INTERSECTION_BATCH_SIZE); + PyModule_AddIntConstant(m, "MAX_RAY_INTERSECTION_BATCH_SIZE", MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING); PyModule_AddIntConstant(m, "B3G_F1", B3G_F1); PyModule_AddIntConstant(m, "B3G_F2", B3G_F2);