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

PyBullet: deal with backward compatibility of b3RaycastBatchAddRay:

Browse Source 
use b3RaycastBatchAddRays API to enable MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING num rays.
Old API (b3RaycastBatchAddRay) sticks to 256 rays, MAX_RAY_INTERSECTION_BATCH_SIZE.
This commit is contained in:
erwincoumans
2018-06-16 12:28:21 -07:00
parent 04d03d10be
commit 7924d51665
8 changed files with 85 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2736,12 +2736,18 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsCl
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_numRays = 0; command->m_requestRaycastIntersections.m_numCommandRays = 1;
command->m_requestRaycastIntersections.m_numStreamingRays = 0;
command->m_requestRaycastIntersections.m_numThreads = 1; command->m_requestRaycastIntersections.m_numThreads = 1;
double rayFrom[3] = {rayFromWorldX,rayFromWorldY,rayFromWorldZ}; command->m_requestRaycastIntersections.m_numCommandRays = 1;
double rayTo[3] = {rayToWorldX,rayToWorldY,rayToWorldZ}; command->m_requestRaycastIntersections.m_fromToRays[0].m_rayFromPosition[0] = rayFromWorldX;
cl->uploadRaysToSharedMemory(*command, rayFrom, rayTo, 1); 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; return (b3SharedMemoryCommandHandle)command;
} }
@@ -2755,7 +2761,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3Phys
b3Assert(command); b3Assert(command);
command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS; command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
command->m_updateFlags = 0; 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; command->m_requestRaycastIntersections.m_numThreads = 1;
return (b3SharedMemoryCommandHandle)command; return (b3SharedMemoryCommandHandle)command;
} }
@@ -2767,18 +2774,26 @@ B3_SHARED_API void b3RaycastBatchSetNumThreads(b3SharedMemoryCommandHandle comm
command->m_requestRaycastIntersections.m_numThreads = numThreads; 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; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS); b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
if (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 (numRays<MAX_RAY_INTERSECTION_BATCH_SIZE)
{
command->m_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; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS); b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
b3Assert(numRays<MAX_RAY_INTERSECTION_BATCH_SIZE); b3Assert(numRays<MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING);
if (command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS) if (command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS)
{ {
cl->uploadRaysToSharedMemory(*command, rayFromWorldArray, rayToWorldArray, numRays); cl->uploadRaysToSharedMemory(*command, rayFromWorldArray, rayToWorldArray, numRays);

4
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -544,7 +544,9 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsCl
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient); 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. // 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 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 b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double* rayFromWorld, const double* rayToWorld, int numRays);
B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo); B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo);

22
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -1719,22 +1719,22 @@ void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data,
void PhysicsClientSharedMemory::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) void PhysicsClientSharedMemory::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays)
{ {
int curNumRays = command.m_requestRaycastIntersections.m_numRays; int curNumStreamingRays = command.m_requestRaycastIntersections.m_numStreamingRays;
int newNumRays = curNumRays + numRays; int newNumRays = curNumStreamingRays + numRays;
btAssert(newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE); btAssert(newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING);
if (newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE) if (newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING)
{ {
for (int i=0;i<numRays;i++) for (int i=0;i<numRays;i++)
{ {
b3RayData* rayDataStream = (b3RayData *)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; b3RayData* rayDataStream = (b3RayData *)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
rayDataStream[curNumRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0]; rayDataStream[curNumStreamingRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0];
rayDataStream[curNumRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1]; rayDataStream[curNumStreamingRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1];
rayDataStream[curNumRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2]; rayDataStream[curNumStreamingRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2];
rayDataStream[curNumRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0]; rayDataStream[curNumStreamingRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0];
rayDataStream[curNumRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1]; rayDataStream[curNumStreamingRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1];
rayDataStream[curNumRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2]; rayDataStream[curNumStreamingRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2];
command.m_requestRaycastIntersections.m_numRays++; command.m_requestRaycastIntersections.m_numStreamingRays++;
} }
} }

22
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -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) void PhysicsDirect::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays)
{ {
int curNumRays = command.m_requestRaycastIntersections.m_numRays; int curNumStreamingRays = command.m_requestRaycastIntersections.m_numStreamingRays;
int newNumRays = curNumRays + numRays; int newNumRays = curNumStreamingRays + numRays;
btAssert(newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE); btAssert(newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING);
if (newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE) if (newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING)
{ {
for (int i=0;i<numRays;i++) for (int i=0;i<numRays;i++)
{ {
b3RayData* rayDataStream = (b3RayData *)m_data->m_bulletStreamDataServerToClient; b3RayData* rayDataStream = (b3RayData *)m_data->m_bulletStreamDataServerToClient;
rayDataStream[curNumRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0]; rayDataStream[curNumStreamingRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0];
rayDataStream[curNumRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1]; rayDataStream[curNumStreamingRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1];
rayDataStream[curNumRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2]; rayDataStream[curNumStreamingRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2];
rayDataStream[curNumRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0]; rayDataStream[curNumStreamingRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0];
rayDataStream[curNumRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1]; rayDataStream[curNumStreamingRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1];
rayDataStream[curNumRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2]; rayDataStream[curNumStreamingRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2];
command.m_requestRaycastIntersections.m_numRays++; command.m_requestRaycastIntersections.m_numStreamingRays++;
} }
} }

23
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -4826,21 +4826,30 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co
BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS"); BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS");
serverStatusOut.m_raycastHits.m_numRaycastHits = 0; 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; const int numThreads = clientCmd.m_requestRaycastIntersections.m_numThreads;
int totalRays = numCommandRays+numStreamingRays;
btAlignedObjectArray<b3RayData> rays; btAlignedObjectArray<b3RayData> rays;
rays.resize(numRays); rays.resize(totalRays);
memcpy(&rays[0],bufferServerToClient,numRays*sizeof(b3RayData)); 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) { if (numThreads == 0) {
// When 0 is specified, Bullet can decide how many threads to use. // When 0 is specified, Bullet can decide how many threads to use.
// About 16 rays per thread seems to work reasonably well. // About 16 rays per thread seems to work reasonably well.
batchRayCaster.castRays(numRays / 16); batchRayCaster.castRays(totalRays / 16);
} else if (numThreads == 1) { } else if (numThreads == 1) {
// Sequentially trace all rays: // Sequentially trace all rays:
for (int i = 0; i < numRays; i++) { for (int i = 0; i < totalRays; i++) {
batchRayCaster.processRay(i); batchRayCaster.processRay(i);
} }
} else { } else {
@@ -4849,7 +4858,7 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co
batchRayCaster.castRays(numThreads); batchRayCaster.castRays(numThreads);
} }
serverStatusOut.m_raycastHits.m_numRaycastHits = numRays; serverStatusOut.m_raycastHits.m_numRaycastHits = totalRays;
serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED; serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED;
return hasStatus; return hasStatus;
} }

10
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -287,14 +287,18 @@ struct RequestRaycastIntersections
// 1: Use a single thread (i.e. no multi-threading) // 1: Use a single thread (i.e. no multi-threading)
// 2 or more: Number of threads to use. // 2 or more: Number of threads to use.
int m_numThreads; int m_numThreads;
int m_numRays; int m_numCommandRays;
// Actual ray data stored in m_bulletStreamDataServerToClientRefactor. //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 struct SendRaycastHits
{ {
int m_numRaycastHits; int m_numRaycastHits;
// Actual ray data stored in m_bulletStreamDataServerToClientRefactor. // Actual ray result data stored in shared memory streaming part.
}; };
struct RequestContactDataArgs struct RequestContactDataArgs

5
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -582,7 +582,10 @@ typedef union {
struct b3RayData a; struct b3RayData a;
struct b3RayHitInfo b; struct b3RayHitInfo b;
} RAY_DATA_UNION; } 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 MAX_RAY_HITS MAX_RAY_INTERSECTION_BATCH_SIZE
#define VISUAL_SHAPE_MAX_PATH_LEN 1024 #define VISUAL_SHAPE_MAX_PATH_LEN 1024

8
examples/pybullet/pybullet.c
View File

@@ -4730,7 +4730,7 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
{ {
int i; 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."); PyErr_SetString(SpamError, "Number of rays exceed the maximum batch size.");
Py_DECREF(seqRayFromObj); Py_DECREF(seqRayFromObj);
@@ -4748,7 +4748,9 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
if ((pybullet_internalSetVectord(rayFromObj, rayFromWorld)) && if ((pybullet_internalSetVectord(rayFromObj, rayFromWorld)) &&
(pybullet_internalSetVectord(rayToObj, rayToWorld))) (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 } else
{ {
PyErr_SetString(SpamError, "Items in the from/to positions need to be an [x,y,z] list of 3 floats/doubles"); 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_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, "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_F1", B3G_F1);
PyModule_AddIntConstant(m, "B3G_F2", B3G_F2); PyModule_AddIntConstant(m, "B3G_F2", B3G_F2);