diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 6d34f8f53..6dd537fd6 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -43,8 +43,9 @@ public: virtual void setSharedMemoryKey(int key) = 0; virtual void uploadBulletFileToSharedMemory(const char* data, int len) = 0; - virtual char* getSharedMemoryStreamBuffer() = 0; - + + virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) = 0; + virtual int getNumDebugLines() const = 0; virtual const float* getDebugLinesFrom() const = 0; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 5e9707094..ab28ab411 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2736,16 +2736,13 @@ 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 = 1; + command->m_requestRaycastIntersections.m_numRays = 0; command->m_requestRaycastIntersections.m_numThreads = 1; - b3RayData* rayDataStream = (b3RayData *)cl->getSharedMemoryStreamBuffer(); - rayDataStream[0].m_rayFromPosition[0] = rayFromWorldX; - rayDataStream[0].m_rayFromPosition[1] = rayFromWorldY; - rayDataStream[0].m_rayFromPosition[2] = rayFromWorldZ; - rayDataStream[0].m_rayToPosition[0] = rayToWorldX; - rayDataStream[0].m_rayToPosition[1] = rayToWorldY; - rayDataStream[0].m_rayToPosition[2] = rayToWorldZ; - + + double rayFrom[3] = {rayFromWorldX,rayFromWorldY,rayFromWorldZ}; + double rayTo[3] = {rayToWorldX,rayToWorldY,rayToWorldZ}; + cl->uploadRaysToSharedMemory(*command, rayFrom, rayTo, 1); + return (b3SharedMemoryCommandHandle)command; } @@ -2770,31 +2767,37 @@ B3_SHARED_API void b3RaycastBatchSetNumThreads(b3SharedMemoryCommandHandle comm command->m_requestRaycastIntersections.m_numThreads = numThreads; } -B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]) +B3_SHARED_API void b3RaycastBatchAddRay(b3PhysicsClientHandle physClient, 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); - b3Assert(command->m_client) - PhysicsClient *cl = command->m_client; - b3Assert(cl); + if (command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS) { - int numRays = command->m_requestRaycastIntersections.m_numRays; - if (numRaysgetSharedMemoryStreamBuffer(); - rayDataStream[numRays].m_rayFromPosition[0] = rayFromWorld[0]; - rayDataStream[numRays].m_rayFromPosition[1] = rayFromWorld[1]; - rayDataStream[numRays].m_rayFromPosition[2] = rayFromWorld[2]; - rayDataStream[numRays].m_rayToPosition[0] = rayToWorld[0]; - rayDataStream[numRays].m_rayToPosition[1] = rayToWorld[1]; - rayDataStream[numRays].m_rayToPosition[2] = rayToWorld[2]; - command->m_requestRaycastIntersections.m_numRays++; - } + cl->uploadRaysToSharedMemory(*command, rayFromWorld, rayToWorld, 1); } } +B3_SHARED_API void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + + 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); + } +} + + B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index ee71d8b97..6146a452e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -544,7 +544,8 @@ 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(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[/*3*/], const double rayToWorld[/*3*/]); +B3_SHARED_API void b3RaycastBatchAddRay(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]); +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 50abe7905..63cccdf7e 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1671,7 +1671,6 @@ bool PhysicsClientSharedMemory::canSubmitCommand() const { struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() { static int sequence = 0; m_data->m_testBlock1->m_clientCommands[0].m_sequenceNumber = sequence++; - m_data->m_testBlock1->m_clientCommands[0].m_client = this; return &m_data->m_testBlock1->m_clientCommands[0]; } @@ -1691,9 +1690,6 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c return false; } -char* PhysicsClientSharedMemory::getSharedMemoryStreamBuffer() { - return m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; -} void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, int len) { btAssert(len < SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); @@ -1709,6 +1705,31 @@ 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++; + } + + } +} + + + void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* cameraData) { cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index a611e9eb9..10c1c63fb 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -54,7 +54,8 @@ public: virtual void setSharedMemoryKey(int key); virtual void uploadBulletFileToSharedMemory(const char* data, int len); - virtual char* getSharedMemoryStreamBuffer(); + + virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays); virtual int getNumDebugLines() const; diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index bebfb158d..aba9264ed 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -112,7 +112,7 @@ PhysicsDirect::PhysicsDirect(PhysicsCommandProcessorInterface* physSdk, bool pas m_data = new PhysicsDirectInternalData; m_data->m_commandProcessor = physSdk; m_data->m_ownsCommandProcessor = passSdkOwnership; - m_data->m_command.m_client = this; + } PhysicsDirect::~PhysicsDirect() @@ -1330,16 +1330,11 @@ bool PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointIn return false; } -///todo: move this out of the + void PhysicsDirect::setSharedMemoryKey(int key) { - //m_data->m_physicsServer->setSharedMemoryKey(key); - //m_data->m_physicsClient->setSharedMemoryKey(key); } -char* PhysicsDirect::getSharedMemoryStreamBuffer() { - return m_data->m_bulletStreamDataServerToClient; -} void PhysicsDirect::uploadBulletFileToSharedMemory(const char* data, int len) { @@ -1354,6 +1349,31 @@ void PhysicsDirect::uploadBulletFileToSharedMemory(const char* data, int len) //m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,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++; + } + + } + +} + + int PhysicsDirect::getNumDebugLines() const { return m_data->m_debugLinesFrom.size(); diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index f12f917a6..2b4a9d634 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -78,7 +78,8 @@ public: virtual void setSharedMemoryKey(int key); void uploadBulletFileToSharedMemory(const char* data, int len); - virtual char* getSharedMemoryStreamBuffer(); + + virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays); virtual int getNumDebugLines() const; diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index cff52a534..07e5e2ca3 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -140,15 +140,17 @@ void PhysicsLoopBack::setSharedMemoryKey(int key) m_data->m_physicsClient->setSharedMemoryKey(key); } -char* PhysicsLoopBack::getSharedMemoryStreamBuffer() { - return m_data->m_physicsClient->getSharedMemoryStreamBuffer(); -} void PhysicsLoopBack::uploadBulletFileToSharedMemory(const char* data, int len) { m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len); } +void PhysicsLoopBack::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) +{ + m_data->m_physicsClient->uploadRaysToSharedMemory(command, rayFromWorldArray, rayToWorldArray, numRays); +} + int PhysicsLoopBack::getNumDebugLines() const { return m_data->m_physicsClient->getNumDebugLines(); diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index 512ddf7ba..5824431a5 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -58,7 +58,8 @@ public: virtual void setSharedMemoryKey(int key); void uploadBulletFileToSharedMemory(const char* data, int len); - virtual char* getSharedMemoryStreamBuffer(); + + virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays); virtual int getNumDebugLines() const; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 3c42850a9..6df03e6f4 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -23,6 +23,12 @@ typedef unsigned long long int smUint64_t; #endif +#ifdef __APPLE__ + #define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (512*1024) +#else + #define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (8*1024*1024) +#endif + #define SHARED_MEMORY_SERVER_TEST_C #define MAX_DEGREE_OF_FREEDOM 128 #define MAX_NUM_SENSORS 256 @@ -1011,7 +1017,7 @@ struct SharedMemoryCommand int m_type; smUint64_t m_timeStamp; int m_sequenceNumber; - struct PhysicsClient *m_client; + //m_updateFlags is a bit fields to tell which parameters need updating //for example m_updateFlags = SIM_PARAM_UPDATE_DELTA_TIME | SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 5a57d69e5..f6467fb38 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -4,8 +4,11 @@ #define SHARED_MEMORY_KEY 12347 ///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures ///my convention is year/month/day/rev +//Please don't replace an existing magic number: +//instead, only ADD a new one at the top, comment-out previous one #define SHARED_MEMORY_MAGIC_NUMBER 201806150 +//#define SHARED_MEMORY_MAGIC_NUMBER 201806020 //#define SHARED_MEMORY_MAGIC_NUMBER 201801170 //#define SHARED_MEMORY_MAGIC_NUMBER 201801080 //#define SHARED_MEMORY_MAGIC_NUMBER 201801010 @@ -17,11 +20,7 @@ //#define SHARED_MEMORY_MAGIC_NUMBER 201706001 //#define SHARED_MEMORY_MAGIC_NUMBER 201703024 -#ifdef __APPLE__ - #define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (512*1024) -#else - #define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (8*1024*1024) -#endif + enum EnumSharedMemoryClientCommand { @@ -583,7 +582,7 @@ typedef union { struct b3RayData a; struct b3RayHitInfo b; } RAY_DATA_UNION; -#define MAX_RAY_INTERSECTION_BATCH_SIZE SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE / sizeof( RAY_DATA_UNION ) +#define MAX_RAY_INTERSECTION_BATCH_SIZE 16*1024 #define MAX_RAY_HITS MAX_RAY_INTERSECTION_BATCH_SIZE #define VISUAL_SHAPE_MAX_PATH_LEN 1024 diff --git a/examples/pybullet/examples/batchRayCast.py b/examples/pybullet/examples/batchRayCast.py index 1862c6918..f25cf4cff 100644 --- a/examples/pybullet/examples/batchRayCast.py +++ b/examples/pybullet/examples/batchRayCast.py @@ -2,9 +2,15 @@ import pybullet as p import time import math -p.connect(p.DIRECT)#GUI) +useGui = True + +if (useGui): + p.connect(p.GUI) +else: + p.connect(p.DIRECT) + +#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) -p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) p.loadURDF("samurai.urdf") p.loadURDF("r2d2.urdf",[3,3,1]) @@ -12,7 +18,7 @@ p.loadURDF("r2d2.urdf",[3,3,1]) rayFrom=[] rayTo=[] -numRays = 2048 +numRays = 1024 rayLen = 13 @@ -20,25 +26,38 @@ for i in range (numRays): rayFrom.append([0,0,1]) rayTo.append([rayLen*math.sin(2.*math.pi*float(i)/numRays), rayLen*math.cos(2.*math.pi*float(i)/numRays),1]) -timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"rayCastBench.json") -for i in range (10): +if (not useGui): + timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"rayCastBench.json") + +numSteps = 10 +if (useGui): + numSteps = 327680 + +for i in range (numSteps): p.stepSimulation() for j in range (8): results = p.rayTestBatch(rayFrom,rayTo,j+1) for i in range (10): p.removeAllUserDebugItems() - p.removeAllUserDebugItems() + rayHitColor = [1,0,0] rayMissColor = [0,1,0] - #for i in range (numRays): - # if (results[i][0]<0): - # p.addUserDebugLine(rayFrom[i],rayTo[i], rayMissColor) - # else: - # p.addUserDebugLine(rayFrom[i],rayTo[i], rayHitColor) + if (useGui): + p.removeAllUserDebugItems() + for i in range (numRays): + hitObjectUid=results[i][0] + + + if (hitObjectUid<0): + p.addUserDebugLine(rayFrom[i],rayTo[i], rayMissColor) + else: + hitPosition = results[i][3] + p.addUserDebugLine(rayFrom[i],hitPosition, rayHitColor) #time.sleep(1./240.) -p.stopStateLogging(timingLog) \ No newline at end of file +if (not useGui): + p.stopStateLogging(timingLog) \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 250f43df3..72a730982 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4729,7 +4729,7 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* } else { int i; - + if (lenFrom > MAX_RAY_INTERSECTION_BATCH_SIZE) { PyErr_SetString(SpamError, "Number of rays exceed the maximum batch size."); @@ -4748,7 +4748,7 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* if ((pybullet_internalSetVectord(rayFromObj, rayFromWorld)) && (pybullet_internalSetVectord(rayToObj, rayToWorld))) { - b3RaycastBatchAddRay(commandHandle, rayFromWorld, rayToWorld); + b3RaycastBatchAddRay(sm, 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"); diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 692fb8b4d..1282ea96a 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -21,6 +21,7 @@ subject to the following restrictions: #include "btSequentialImpulseConstraintSolver.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" + #include "LinearMath/btIDebugDraw.h" #include "LinearMath/btCpuFeatureUtility.h" @@ -780,8 +781,12 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& } else { + bool isMultiBodyType = (body.getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK); // Incorrectly set collision object flags can degrade performance in various ways. - //btAssert( body.isStaticOrKinematicObject() ); + if (!isMultiBodyType) + { + btAssert( body.isStaticOrKinematicObject() ); + } //it could be a multibody link collider // all fixed bodies (inf mass) get mapped to a single solver id if ( m_fixedBodyId < 0 )