From 53b65b795f543c2bb4a3b10233a07270b936fe5e Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 9 May 2020 02:42:47 -0700 Subject: [PATCH] add fractionEpsilon to filter rays with similar hit fraction remove debug printf --- examples/SharedMemory/PhysicsClientC_API.cpp | 12 ++++++ examples/SharedMemory/PhysicsClientC_API.h | 2 +- .../PhysicsServerCommandProcessor.cpp | 40 ++++++++++++++----- examples/SharedMemory/SharedMemoryCommands.h | 1 + examples/pybullet/pybullet.c | 12 ++++-- 5 files changed, 53 insertions(+), 14 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 705ace6f5..e65a7be93 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3652,6 +3652,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsCl command->m_requestRaycastIntersections.m_fromToRays[0].m_rayToPosition[2] = rayToWorldZ; command->m_requestRaycastIntersections.m_reportHitNumber = -1; command->m_requestRaycastIntersections.m_collisionFilterMask = -1; + command->m_requestRaycastIntersections.m_fractionEpsilon = B3_EPSILON; + return (b3SharedMemoryCommandHandle)command; @@ -3673,6 +3675,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3Phys command->m_requestRaycastIntersections.m_parentLinkIndex=-1; command->m_requestRaycastIntersections.m_reportHitNumber = -1; command->m_requestRaycastIntersections.m_collisionFilterMask = -1; + command->m_requestRaycastIntersections.m_fractionEpsilon = B3_EPSILON; return (b3SharedMemoryCommandHandle)command; } @@ -3747,6 +3750,15 @@ B3_SHARED_API void b3RaycastBatchSetCollisionFilterMask(b3SharedMemoryCommandHan command->m_requestRaycastIntersections.m_collisionFilterMask = collisionFilterMask; } +B3_SHARED_API void b3RaycastBatchSetFractionEpsilon(b3SharedMemoryCommandHandle commandHandle, double fractionEpsilon) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS); + command->m_requestRaycastIntersections.m_fractionEpsilon = fractionEpsilon; +} + + B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo) { PhysicsClient* cl = (PhysicsClient*)physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index aed01a631..41d1a9fba 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -627,7 +627,7 @@ extern "C" 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 b3RaycastBatchSetFractionEpsilon(b3SharedMemoryCommandHandle commandHandle, double fractionEpsilon); B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 9df45808d..309fbc574 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5961,13 +5961,15 @@ struct FilteredClosestRayResultCallback : public btCollisionWorld::ClosestRayRes struct FilteredAllHitsRayResultCallback: public btCollisionWorld::AllHitsRayResultCallback { - FilteredAllHitsRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, int collisionFilterMask) + FilteredAllHitsRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, int collisionFilterMask, btScalar fractionEpsilon) : btCollisionWorld::AllHitsRayResultCallback(rayFromWorld, rayToWorld), - m_collisionFilterMask(collisionFilterMask) + m_collisionFilterMask(collisionFilterMask), + m_fractionEpsilon(fractionEpsilon) { } int m_collisionFilterMask; + btScalar m_fractionEpsilon; virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) { @@ -5977,6 +5979,25 @@ struct FilteredAllHitsRayResultCallback: public btCollisionWorld::AllHitsRayResu if (!collides) return m_closestHitFraction; } + //remove duplicate hits: + //same collision object, link index and hit fraction + bool isDuplicate = false; + + for (int i = 0; i < m_collisionObjects.size(); i++) + { + if (m_collisionObjects[i] == rayResult.m_collisionObject) + { + btScalar diffFraction = m_hitFractions[i]-rayResult.m_hitFraction; + if (btEqual(diffFraction, m_fractionEpsilon)) + { + isDuplicate = true; + break; + } + } + } + if (isDuplicate) + return m_closestHitFraction; + return btCollisionWorld::AllHitsRayResultCallback::addSingleResult(rayResult, normalInWorldSpace); } }; @@ -5991,10 +6012,12 @@ struct BatchRayCaster int m_numRays; int m_reportHitNumber; int m_collisionFilterMask; + btScalar m_fractionEpsilon; - BatchRayCaster(b3ThreadPool* threadPool, const btCollisionWorld* world, const b3RayData* rayInputBuffer, b3RayHitInfo* hitInfoOutputBuffer, int numRays, int reportHitNumber, int collisionFilterMask) + BatchRayCaster(b3ThreadPool* threadPool, const btCollisionWorld* world, const b3RayData* rayInputBuffer, b3RayHitInfo* hitInfoOutputBuffer, int numRays, int reportHitNumber, int collisionFilterMask, btScalar fractionEpsilon) : m_threadPool(threadPool), m_world(world), m_rayInputBuffer(rayInputBuffer), m_hitInfoOutputBuffer(hitInfoOutputBuffer), m_numRays(numRays), m_reportHitNumber(reportHitNumber), - m_collisionFilterMask(collisionFilterMask) + m_collisionFilterMask(collisionFilterMask), + m_fractionEpsilon(fractionEpsilon) { m_syncInfo = new CastSyncInfo; } @@ -6051,10 +6074,6 @@ struct BatchRayCaster { for (int i = 0; i < m_numRays; i++) { - if (i == 17) - { - printf("!\n"); - } processRay(i); } } @@ -6072,7 +6091,7 @@ struct BatchRayCaster if (m_reportHitNumber >= 0) { //compute all hits, and select the m_reportHitNumber, if available - FilteredAllHitsRayResultCallback allResultsCallback(rayFromWorld, rayToWorld, m_collisionFilterMask); + FilteredAllHitsRayResultCallback allResultsCallback(rayFromWorld, rayToWorld, m_collisionFilterMask, m_fractionEpsilon); allResultsCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest; m_world->rayTest(rayFromWorld, rayToWorld, allResultsCallback); if (allResultsCallback.m_collisionObjects.size() > m_reportHitNumber) @@ -6165,6 +6184,7 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co int numThreads = clientCmd.m_requestRaycastIntersections.m_numThreads; int reportHitNumber = clientCmd.m_requestRaycastIntersections.m_reportHitNumber; int collisionFilterMask = clientCmd.m_requestRaycastIntersections.m_collisionFilterMask; + btScalar fractionEpsilon = clientCmd.m_requestRaycastIntersections.m_fractionEpsilon; if (numThreads == 0) { // When 0 is specified, Bullet can decide how many threads to use. @@ -6233,7 +6253,7 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co } } - BatchRayCaster batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays, reportHitNumber, collisionFilterMask); + BatchRayCaster batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays, reportHitNumber, collisionFilterMask, fractionEpsilon); batchRayCaster.castRays(numThreads); serverStatusOut.m_numDataStreamBytes = totalRays * sizeof(b3RayData); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 9a4b7f41b..3ab8776c9 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -306,6 +306,7 @@ struct RequestRaycastIntersections int m_parentLinkIndex; int m_reportHitNumber; int m_collisionFilterMask; + double m_fractionEpsilon; //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 2ae826586..e5c97d7fd 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -6565,12 +6565,13 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* int parentObjectUniqueId = -1; int parentLinkIndex = -1; int collisionFilterMask = -1; + double fractionEpsilon = -1; - static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "parentObjectUniqueId", "parentLinkIndex", "reportHitNumber", "collisionFilterMask","physicsClientId", NULL}; + static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "parentObjectUniqueId", "parentLinkIndex", "reportHitNumber", "collisionFilterMask","fractionEpsilon","physicsClientId", NULL}; int physicsClientId = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|iiiiii", kwlist, - &rayFromObjList, &rayToObjList, &numThreads, &parentObjectUniqueId, &parentLinkIndex, &reportHitNumber, &collisionFilterMask , &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|iiiiidi", kwlist, + &rayFromObjList, &rayToObjList, &numThreads, &parentObjectUniqueId, &parentLinkIndex, &reportHitNumber, &collisionFilterMask , &fractionEpsilon, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); @@ -6666,6 +6667,11 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* { b3RaycastBatchSetCollisionFilterMask(commandHandle, collisionFilterMask); } + if (fractionEpsilon >= 0) + { + b3RaycastBatchSetFractionEpsilon(commandHandle, fractionEpsilon); + + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED)