AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 13:20:07 +00:00
Code Issues Releases Wiki Activity

add fractionEpsilon to filter rays with similar hit fraction

Browse Source 
remove debug printf
This commit is contained in:
Erwin Coumans 2020-05-09 02:42:47 -07:00
parent b154bf75b8
commit 53b65b795f
5 changed files with 53 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
examples/SharedMemory/PhysicsClientC_API.cpp
View File

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

2
examples/SharedMemory/PhysicsClientC_API.h
View File

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

40
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

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

1
examples/SharedMemory/SharedMemoryCommands.h
View File

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

12
examples/pybullet/pybullet.c
View File

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