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();
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 (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;
b3Assert(command);
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)
{
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);
// 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);

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)
{
int curNumRays = command.m_requestRaycastIntersections.m_numRays;
int newNumRays = curNumRays + numRays;
btAssert(newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE);
int curNumStreamingRays = command.m_requestRaycastIntersections.m_numStreamingRays;
int newNumRays = curNumStreamingRays + numRays;
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++)
{
b3RayData* rayDataStream = (b3RayData *)m_data->m_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++;
}
}

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)
{
int curNumRays = command.m_requestRaycastIntersections.m_numRays;
int newNumRays = curNumRays + numRays;
btAssert(newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE);
int curNumStreamingRays = command.m_requestRaycastIntersections.m_numStreamingRays;
int newNumRays = curNumStreamingRays + numRays;
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++)
{
b3RayData* rayDataStream = (b3RayData *)m_data->m_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++;
}
}

23
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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<b3RayData> 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;
}

10
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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

5
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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

8
examples/pybullet/pybullet.c
View File

@@ -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);