diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index fb17341c3..4b557bcd4 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2898,6 +2898,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3Phys command->m_requestRaycastIntersections.m_numCommandRays = 0; command->m_requestRaycastIntersections.m_numStreamingRays = 0; command->m_requestRaycastIntersections.m_numThreads = 1; + command->m_requestRaycastIntersections.m_parentObjectUniqueId = -1; + command->m_requestRaycastIntersections.m_parentLinkIndex=-1; return (b3SharedMemoryCommandHandle)command; } @@ -2947,6 +2949,16 @@ B3_SHARED_API void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3Sha } } +B3_SHARED_API void b3RaycastBatchSetParentObject(b3SharedMemoryCommandHandle commandHandle, int parentObjectUniqueId, int parentLinkIndex) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS); + command->m_requestRaycastIntersections.m_parentObjectUniqueId = parentObjectUniqueId; + command->m_requestRaycastIntersections.m_parentLinkIndex = parentLinkIndex; +} + + 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 998bf7677..795013e43 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -564,6 +564,7 @@ extern "C" B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]); //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 b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index eb2b4e747..12c1aa7be 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5050,6 +5050,53 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co memcpy(&rays[numCommandRays], bufferServerToClient, numStreamingRays * sizeof(b3RayData)); } + if (clientCmd.m_requestRaycastIntersections.m_parentObjectUniqueId>=0) + { + btTransform tr; + tr.setIdentity(); + + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_requestRaycastIntersections.m_parentObjectUniqueId); + if (bodyHandle) + { + int linkIndex = -1; + if (bodyHandle->m_multiBody) + { + int linkIndex = clientCmd.m_userDebugDrawArgs.m_parentLinkIndex; + if (linkIndex == -1) + { + tr = bodyHandle->m_multiBody->getBaseWorldTransform(); + } + else + { + if (linkIndex >= 0 && linkIndex < bodyHandle->m_multiBody->getNumLinks()) + { + tr = bodyHandle->m_multiBody->getLink(linkIndex).m_cachedWorldTransform; + } + } + } + if (bodyHandle->m_rigidBody) + { + tr = bodyHandle->m_rigidBody->getWorldTransform(); + } + //convert all rays into world space + for (int i=0;im_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays); batchRayCaster.castRays(numThreads); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index d12475707..93292ee89 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1239,10 +1239,35 @@ public: m_tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2]; m_tmpLine.m_trackingVisualShapeIndex = trackingVisualShapeIndex; m_tmpLine.m_replaceItemUid = replaceItemUid; - m_cs->lock(); - m_cs->setSharedParam(1, eGUIUserDebugAddLine); - m_resultDebugLineUid = -1; - workerThreadWait(); + + //don't block when replacing an item + if (replaceItemUid>=0 && replaceItemUid=0) + { + m_userDebugLines[slot] = m_tmpLine; + } + m_resultDebugLineUid = replaceItemUid; + } + else + { + + m_cs->lock(); + m_cs->setSharedParam(1, eGUIUserDebugAddLine); + m_resultDebugLineUid = -1; + workerThreadWait(); + } return m_resultDebugLineUid; } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 45b9c7bf2..1504a69f6 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -293,6 +293,9 @@ struct RequestRaycastIntersections b3RayData m_fromToRays[MAX_RAY_INTERSECTION_BATCH_SIZE]; int m_numStreamingRays; + //optional m_parentObjectUniqueId (-1 for unused) + int m_parentObjectUniqueId; + int m_parentLinkIndex; //streaming ray data stored in shared memory streaming part. (size m_numStreamingRays ) }; diff --git a/examples/pybullet/examples/inverse_kinematics.py b/examples/pybullet/examples/inverse_kinematics.py index c311c3ca1..514829a31 100644 --- a/examples/pybullet/examples/inverse_kinematics.py +++ b/examples/pybullet/examples/inverse_kinematics.py @@ -48,7 +48,7 @@ p.setRealTimeSimulation(useRealTimeSimulation) trailDuration = 15 while 1: - p.getCameraImage(320,200, flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX, renderer=p.ER_HARDWARE_OPENGL_RENDERER) + p.getCameraImage(320,200, flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX, renderer=p.ER_BULLET_HARDWARE_OPENGL) if (useRealTimeSimulation): dt = datetime.now() t = (dt.second/60.)*2.*math.pi diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 798131c46..e1f37edda 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4742,12 +4742,14 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* b3PhysicsClientHandle sm = 0; int sizeFrom = 0; int sizeTo = 0; - - static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "physicsClientId", NULL}; + int parentObjectUniqueId = -1; + int parentLinkIndex = -1; + + static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "parentObjectUniqueId", "parentLinkIndex", "physicsClientId", NULL}; int physicsClientId = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|ii", kwlist, - &rayFromObjList, &rayToObjList, &numThreads, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|iiii", kwlist, + &rayFromObjList, &rayToObjList, &numThreads, &parentObjectUniqueId, &parentLinkIndex, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); @@ -4831,6 +4833,11 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* } } + if (parentObjectUniqueId>=0) + { + b3RaycastBatchSetParentObject(commandHandle, parentObjectUniqueId,parentLinkIndex); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED)