diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 2db4775d6..705ace6f5 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3651,6 +3651,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsCl 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; + command->m_requestRaycastIntersections.m_collisionFilterMask = -1; + return (b3SharedMemoryCommandHandle)command; } @@ -3670,6 +3672,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3Phys command->m_requestRaycastIntersections.m_parentObjectUniqueId = -1; command->m_requestRaycastIntersections.m_parentLinkIndex=-1; command->m_requestRaycastIntersections.m_reportHitNumber = -1; + command->m_requestRaycastIntersections.m_collisionFilterMask = -1; return (b3SharedMemoryCommandHandle)command; } @@ -3736,7 +3739,13 @@ B3_SHARED_API void b3RaycastBatchSetReportHitNumber(b3SharedMemoryCommandHandle b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS); command->m_requestRaycastIntersections.m_reportHitNumber= reportHitNumber; } - +B3_SHARED_API void b3RaycastBatchSetCollisionFilterMask(b3SharedMemoryCommandHandle commandHandle, int collisionFilterMask) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS); + command->m_requestRaycastIntersections.m_collisionFilterMask = collisionFilterMask; +} 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 c522ed34c..aed01a631 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -626,6 +626,8 @@ extern "C" 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 b3RaycastBatchSetCollisionFilterMask(b3SharedMemoryCommandHandle commandHandle, int collisionFilterMask); + B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 80a583da0..9df45808d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5934,6 +5934,53 @@ struct CastSyncInfo }; #endif // __cplusplus >= 201103L + + + +struct FilteredClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback +{ + FilteredClosestRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, int collisionFilterMask) + : btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld), + m_collisionFilterMask(collisionFilterMask) + { + } + + int m_collisionFilterMask; + + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) + { + if (m_collisionFilterMask >= 0) + { + bool collides = (rayResult.m_collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & m_collisionFilterMask) != 0; + if (!collides) + return m_closestHitFraction; + } + return btCollisionWorld::ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace); + } +}; + +struct FilteredAllHitsRayResultCallback: public btCollisionWorld::AllHitsRayResultCallback +{ + FilteredAllHitsRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, int collisionFilterMask) + : btCollisionWorld::AllHitsRayResultCallback(rayFromWorld, rayToWorld), + m_collisionFilterMask(collisionFilterMask) + { + } + + int m_collisionFilterMask; + + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) + { + if (m_collisionFilterMask >= 0) + { + bool collides = (rayResult.m_collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & m_collisionFilterMask) != 0; + if (!collides) + return m_closestHitFraction; + } + return btCollisionWorld::AllHitsRayResultCallback::addSingleResult(rayResult, normalInWorldSpace); + } +}; + struct BatchRayCaster { b3ThreadPool* m_threadPool; @@ -5943,9 +5990,11 @@ struct BatchRayCaster b3RayHitInfo* m_hitInfoOutputBuffer; int m_numRays; int m_reportHitNumber; + int m_collisionFilterMask; - 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) + BatchRayCaster(b3ThreadPool* threadPool, const btCollisionWorld* world, const b3RayData* rayInputBuffer, b3RayHitInfo* hitInfoOutputBuffer, int numRays, int reportHitNumber, int collisionFilterMask) + : m_threadPool(threadPool), m_world(world), m_rayInputBuffer(rayInputBuffer), m_hitInfoOutputBuffer(hitInfoOutputBuffer), m_numRays(numRays), m_reportHitNumber(reportHitNumber), + m_collisionFilterMask(collisionFilterMask) { m_syncInfo = new CastSyncInfo; } @@ -6018,12 +6067,12 @@ struct BatchRayCaster btVector3 rayFromWorld(from[0], from[1], from[2]); btVector3 rayToWorld(to[0], to[1], to[2]); - btCollisionWorld::ClosestRayResultCallback rayResultCallback(rayFromWorld, rayToWorld); + FilteredClosestRayResultCallback rayResultCallback(rayFromWorld, rayToWorld, m_collisionFilterMask); rayResultCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest; if (m_reportHitNumber >= 0) { //compute all hits, and select the m_reportHitNumber, if available - btCollisionWorld::AllHitsRayResultCallback allResultsCallback(rayFromWorld, rayToWorld); + FilteredAllHitsRayResultCallback allResultsCallback(rayFromWorld, rayToWorld, m_collisionFilterMask); allResultsCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest; m_world->rayTest(rayFromWorld, rayToWorld, allResultsCallback); if (allResultsCallback.m_collisionObjects.size() > m_reportHitNumber) @@ -6115,6 +6164,7 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co const int totalRays = numCommandRays + numStreamingRays; int numThreads = clientCmd.m_requestRaycastIntersections.m_numThreads; int reportHitNumber = clientCmd.m_requestRaycastIntersections.m_reportHitNumber; + int collisionFilterMask = clientCmd.m_requestRaycastIntersections.m_collisionFilterMask; if (numThreads == 0) { // When 0 is specified, Bullet can decide how many threads to use. @@ -6183,7 +6233,7 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co } } - BatchRayCaster batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays, reportHitNumber); + BatchRayCaster batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays, reportHitNumber, collisionFilterMask); batchRayCaster.castRays(numThreads); serverStatusOut.m_numDataStreamBytes = totalRays * sizeof(b3RayData); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 6d67981b2..9a4b7f41b 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -305,6 +305,7 @@ struct RequestRaycastIntersections int m_parentObjectUniqueId; int m_parentLinkIndex; int m_reportHitNumber; + int m_collisionFilterMask; //streaming ray data stored in shared memory streaming part. (size m_numStreamingRays ) }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index dc70dd94e..2ae826586 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -6468,11 +6468,12 @@ static PyObject* pybullet_rayTestObsolete(PyObject* self, PyObject* args, PyObje double from[3]; double to[3]; b3PhysicsClientHandle sm = 0; - static char* kwlist[] = {"rayFromPosition", "rayToPosition", "physicsClientId", NULL}; + static char* kwlist[] = {"rayFromPosition", "rayToPosition", "collisionFilterMask", "physicsClientId", NULL}; int physicsClientId = 0; + int collisionFilterMask = -1; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, - &rayFromObj, &rayToObj, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|ii", kwlist, + &rayFromObj, &rayToObj, &collisionFilterMask, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); @@ -6485,9 +6486,16 @@ static PyObject* pybullet_rayTestObsolete(PyObject* self, PyObject* args, PyObje pybullet_internalSetVectord(rayFromObj, from); pybullet_internalSetVectord(rayToObj, to); + commandHandle = b3CreateRaycastCommandInit(sm, from[0], from[1], from[2], to[0], to[1], to[2]); + + if (collisionFilterMask >= 0) + { + b3RaycastBatchSetCollisionFilterMask(commandHandle, collisionFilterMask); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED) @@ -6556,12 +6564,13 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* int sizeTo = 0; int parentObjectUniqueId = -1; int parentLinkIndex = -1; + int collisionFilterMask = -1; - static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "parentObjectUniqueId", "parentLinkIndex", "reportHitNumber", "physicsClientId", NULL}; + static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "parentObjectUniqueId", "parentLinkIndex", "reportHitNumber", "collisionFilterMask","physicsClientId", NULL}; int physicsClientId = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|iiiii", kwlist, - &rayFromObjList, &rayToObjList, &numThreads, &parentObjectUniqueId, &parentLinkIndex, &reportHitNumber, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|iiiiii", kwlist, + &rayFromObjList, &rayToObjList, &numThreads, &parentObjectUniqueId, &parentLinkIndex, &reportHitNumber, &collisionFilterMask , &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); @@ -6653,6 +6662,10 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* { b3RaycastBatchSetReportHitNumber(commandHandle, reportHitNumber); } + if (collisionFilterMask >= 0) + { + b3RaycastBatchSetCollisionFilterMask(commandHandle, collisionFilterMask); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED)