Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

allow to provide rayCastBatch in local 'from'/'to' with a parent/link index, b3RaycastBatchSetParentObject

Browse Source 
If parentObjectUniqueId provided, convert local from/to into world space coordinates
AddUserDebugLins: don't block when replacing an item
Fix examples/pybullet/examples/inverse_kinematics.py
This commit is contained in:
erwincoumans
2018-10-10 23:31:50 -07:00
parent 32b5c88d4b
commit bb305c6ebc
7 changed files with 104 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
examples/SharedMemory/PhysicsClientC_API.cpp
View File

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

1
examples/SharedMemory/PhysicsClientC_API.h
View File

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

47
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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;i<totalRays;i++)
{
btVector3 localPosTo(rays[i].m_rayToPosition[0],rays[i].m_rayToPosition[1],rays[i].m_rayToPosition[2]);
btVector3 worldPosTo = tr*localPosTo;
btVector3 localPosFrom(rays[i].m_rayFromPosition[0],rays[i].m_rayFromPosition[1],rays[i].m_rayFromPosition[2]);
btVector3 worldPosFrom = tr*localPosFrom;
rays[i].m_rayFromPosition[0] = worldPosFrom[0];
rays[i].m_rayFromPosition[1] = worldPosFrom[1];
rays[i].m_rayFromPosition[2] = worldPosFrom[2];
rays[i].m_rayToPosition[0] = worldPosTo[0];
rays[i].m_rayToPosition[1] = worldPosTo[1];
rays[i].m_rayToPosition[2] = worldPosTo[2];
}
}
}
BatchRayCaster batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays);
batchRayCaster.castRays(numThreads);

33
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -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<m_userDebugLines.size())
{
//find the right slot
int slot=-1;
for (int i=0;i<m_userDebugLines.size();i++)
{
if (replaceItemUid == m_userDebugLines[i].m_itemUniqueId)
{
slot = i;
}
}
if (slot>=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;
}

3
examples/SharedMemory/SharedMemoryCommands.h
View File

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

2
examples/pybullet/examples/inverse_kinematics.py
View File

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

13
examples/pybullet/pybullet.c
View File

@@ -4742,12 +4742,14 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
b3PhysicsClientHandle sm = 0;
int sizeFrom = 0;
int sizeTo = 0;
int parentObjectUniqueId = -1;
int parentLinkIndex = -1;
static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "physicsClientId", NULL};
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)