diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index b76c3466f..2db4775d6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3650,6 +3650,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsCl 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; + command->m_requestRaycastIntersections.m_reportHitNumber = -1; return (b3SharedMemoryCommandHandle)command; } @@ -3668,6 +3669,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3Phys command->m_requestRaycastIntersections.m_numThreads = 1; command->m_requestRaycastIntersections.m_parentObjectUniqueId = -1; command->m_requestRaycastIntersections.m_parentLinkIndex=-1; + command->m_requestRaycastIntersections.m_reportHitNumber = -1; + return (b3SharedMemoryCommandHandle)command; } @@ -3726,6 +3729,14 @@ B3_SHARED_API void b3RaycastBatchSetParentObject(b3SharedMemoryCommandHandle com command->m_requestRaycastIntersections.m_parentLinkIndex = parentLinkIndex; } +B3_SHARED_API void b3RaycastBatchSetReportHitNumber(b3SharedMemoryCommandHandle commandHandle, int reportHitNumber) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS); + command->m_requestRaycastIntersections.m_reportHitNumber= reportHitNumber; +} + 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 438a4b3c0..c522ed34c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -625,7 +625,8 @@ extern "C" //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 b3RaycastBatchSetParentObject(b3SharedMemoryCommandHandle commandHandle, int parentObjectUniqueId, int parentLinkIndex); - + B3_SHARED_API void b3RaycastBatchSetReportHitNumber(b3SharedMemoryCommandHandle commandHandle, int reportHitNumber); + B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo); /// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates. diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d84688918..80a583da0 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4993,30 +4993,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); glmesh = LoadMeshFromObj(relativeFileName, pathPrefix, fileIO); } - if (glmesh) - { - if (compound == 0) - { - compound = worldImporter->createCompoundShape(); - } - - btTriangleMesh* meshInterface = new btTriangleMesh(); - m_data->m_meshInterfaces.push_back(meshInterface); - - for (int i = 0; i < glmesh->m_numIndices / 3; i++) - { - float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i * 3)).xyzw; - float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i * 3 + 1)).xyzw; - float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i * 3 + 2)).xyzw; - meshInterface->addTriangle( - btVector3(v0[0], v0[1], v0[2])* meshScale, - btVector3(v1[0], v1[1], v1[2])* meshScale, - btVector3(v2[0], v2[1], v2[2])* meshScale); - } - - btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true); - compound->addChildShape(childTransform, trimesh); - } + //btBvhTriangleMeshShape is created below } else { @@ -5049,6 +5026,8 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str convexHull->optimizeConvexHull(); compound->addChildShape(childTransform, convexHull); + delete glmesh; + glmesh = 0; } if (out_type == UrdfGeometry::FILE_OBJ) { @@ -5963,9 +5942,10 @@ struct BatchRayCaster const b3RayData* m_rayInputBuffer; b3RayHitInfo* m_hitInfoOutputBuffer; int m_numRays; + int m_reportHitNumber; - BatchRayCaster(b3ThreadPool* threadPool, const btCollisionWorld* world, const b3RayData* rayInputBuffer, b3RayHitInfo* hitInfoOutputBuffer, int numRays) - : m_threadPool(threadPool), m_world(world), m_rayInputBuffer(rayInputBuffer), m_hitInfoOutputBuffer(hitInfoOutputBuffer), m_numRays(numRays) + BatchRayCaster(b3ThreadPool* threadPool, const btCollisionWorld* world, const b3RayData* rayInputBuffer, b3RayHitInfo* hitInfoOutputBuffer, int numRays, int reportHitNumber) + : m_threadPool(threadPool), m_world(world), m_rayInputBuffer(rayInputBuffer), m_hitInfoOutputBuffer(hitInfoOutputBuffer), m_numRays(numRays), m_reportHitNumber(reportHitNumber) { m_syncInfo = new CastSyncInfo; } @@ -6022,6 +6002,10 @@ struct BatchRayCaster { for (int i = 0; i < m_numRays; i++) { + if (i == 17) + { + printf("!\n"); + } processRay(i); } } @@ -6036,8 +6020,24 @@ struct BatchRayCaster btCollisionWorld::ClosestRayResultCallback rayResultCallback(rayFromWorld, rayToWorld); rayResultCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest; - - m_world->rayTest(rayFromWorld, rayToWorld, rayResultCallback); + if (m_reportHitNumber >= 0) + { + //compute all hits, and select the m_reportHitNumber, if available + btCollisionWorld::AllHitsRayResultCallback allResultsCallback(rayFromWorld, rayToWorld); + allResultsCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest; + m_world->rayTest(rayFromWorld, rayToWorld, allResultsCallback); + if (allResultsCallback.m_collisionObjects.size() > m_reportHitNumber) + { + rayResultCallback.m_collisionObject = allResultsCallback.m_collisionObjects[m_reportHitNumber]; + rayResultCallback.m_closestHitFraction = allResultsCallback.m_hitFractions[m_reportHitNumber]; + rayResultCallback.m_hitNormalWorld = allResultsCallback.m_hitNormalWorld[m_reportHitNumber]; + rayResultCallback.m_hitPointWorld = allResultsCallback.m_hitPointWorld[m_reportHitNumber]; + } + } + else + { + m_world->rayTest(rayFromWorld, rayToWorld, rayResultCallback); + } b3RayHitInfo& hit = m_hitInfoOutputBuffer[ray]; if (rayResultCallback.hasHit()) @@ -6114,6 +6114,7 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co const int numStreamingRays = clientCmd.m_requestRaycastIntersections.m_numStreamingRays; const int totalRays = numCommandRays + numStreamingRays; int numThreads = clientCmd.m_requestRaycastIntersections.m_numThreads; + int reportHitNumber = clientCmd.m_requestRaycastIntersections.m_reportHitNumber; if (numThreads == 0) { // When 0 is specified, Bullet can decide how many threads to use. @@ -6182,7 +6183,7 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co } } - BatchRayCaster batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays); + BatchRayCaster batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays, reportHitNumber); batchRayCaster.castRays(numThreads); serverStatusOut.m_numDataStreamBytes = totalRays * sizeof(b3RayData); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index af6cb907b..6d67981b2 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -304,6 +304,7 @@ struct RequestRaycastIntersections //optional m_parentObjectUniqueId (-1 for unused) int m_parentObjectUniqueId; int m_parentLinkIndex; + int m_reportHitNumber; //streaming ray data stored in shared memory streaming part. (size m_numStreamingRays ) }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 17f947048..eea99bd3c 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -7,7 +7,8 @@ //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 202002030 +#define SHARED_MEMORY_MAGIC_NUMBER 202005070 +//#define SHARED_MEMORY_MAGIC_NUMBER 202002030 //#define SHARED_MEMORY_MAGIC_NUMBER 202001230 //#define SHARED_MEMORY_MAGIC_NUMBER 201911280 //#define SHARED_MEMORY_MAGIC_NUMBER 201911180 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e65abdf6d..bb47c5948 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -6550,17 +6550,18 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* PyObject* rayFromObjList = 0; PyObject* rayToObjList = 0; int numThreads = 1; + int reportHitNumber = -1; b3PhysicsClientHandle sm = 0; int sizeFrom = 0; int sizeTo = 0; int parentObjectUniqueId = -1; int parentLinkIndex = -1; - static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "parentObjectUniqueId", "parentLinkIndex", "physicsClientId", NULL}; + static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "parentObjectUniqueId", "parentLinkIndex", "reportHitNumber", "physicsClientId", NULL}; int physicsClientId = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|iiii", kwlist, - &rayFromObjList, &rayToObjList, &numThreads, &parentObjectUniqueId, &parentLinkIndex, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|iiiii", kwlist, + &rayFromObjList, &rayToObjList, &numThreads, &parentObjectUniqueId, &parentLinkIndex, &reportHitNumber, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); @@ -6648,7 +6649,10 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* { b3RaycastBatchSetParentObject(commandHandle, parentObjectUniqueId, parentLinkIndex); } - + if (reportHitNumber >= 0) + { + b3RaycastBatchSetReportHitNumber(commandHandle, reportHitNumber); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED)