Implement rayTestBatch. At the moment, it is still testing individual rays on the physics server. We can enable multi-threaded version later. At least the python + shared-memory IPC overhead will be much lower.
This commit is contained in:
@@ -1383,17 +1383,52 @@ b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle phy
|
|||||||
struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
|
struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
|
||||||
b3Assert(command);
|
b3Assert(command);
|
||||||
command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
|
command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
|
||||||
command->m_requestRaycastIntersections.m_rayFromPosition[0] = rayFromWorldX;
|
command->m_requestRaycastIntersections.m_numRays = 1;
|
||||||
command->m_requestRaycastIntersections.m_rayFromPosition[1] = rayFromWorldY;
|
command->m_requestRaycastIntersections.m_rayFromPositions[0][0] = rayFromWorldX;
|
||||||
command->m_requestRaycastIntersections.m_rayFromPosition[2] = rayFromWorldZ;
|
command->m_requestRaycastIntersections.m_rayFromPositions[0][1] = rayFromWorldY;
|
||||||
command->m_requestRaycastIntersections.m_rayToPosition[0] = rayToWorldX;
|
command->m_requestRaycastIntersections.m_rayFromPositions[0][2] = rayFromWorldZ;
|
||||||
command->m_requestRaycastIntersections.m_rayToPosition[1] = rayToWorldY;
|
command->m_requestRaycastIntersections.m_rayToPositions[0][0] = rayToWorldX;
|
||||||
command->m_requestRaycastIntersections.m_rayToPosition[2] = rayToWorldZ;
|
command->m_requestRaycastIntersections.m_rayToPositions[0][1] = rayToWorldY;
|
||||||
|
command->m_requestRaycastIntersections.m_rayToPositions[0][2] = rayToWorldZ;
|
||||||
|
|
||||||
return (b3SharedMemoryCommandHandle)command;
|
return (b3SharedMemoryCommandHandle)command;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient)
|
||||||
|
{
|
||||||
|
PhysicsClient *cl = (PhysicsClient *)physClient;
|
||||||
|
b3Assert(cl);
|
||||||
|
b3Assert(cl->canSubmitCommand());
|
||||||
|
struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
|
||||||
|
b3Assert(command);
|
||||||
|
command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
|
||||||
|
command->m_updateFlags = 0;
|
||||||
|
command->m_requestRaycastIntersections.m_numRays = 0;
|
||||||
|
return (b3SharedMemoryCommandHandle)command;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3])
|
||||||
|
{
|
||||||
|
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||||
|
b3Assert(command);
|
||||||
|
b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
|
||||||
|
if (command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS)
|
||||||
|
{
|
||||||
|
int numRays = command->m_requestRaycastIntersections.m_numRays;
|
||||||
|
if (numRays<MAX_RAY_INTERSECTION_BATCH_SIZE)
|
||||||
|
{
|
||||||
|
command->m_requestRaycastIntersections.m_rayFromPositions[numRays][0] = rayFromWorld[0];
|
||||||
|
command->m_requestRaycastIntersections.m_rayFromPositions[numRays][1] = rayFromWorld[1];
|
||||||
|
command->m_requestRaycastIntersections.m_rayFromPositions[numRays][2] = rayFromWorld[2];
|
||||||
|
command->m_requestRaycastIntersections.m_rayToPositions[numRays][0] = rayToWorld[0];
|
||||||
|
command->m_requestRaycastIntersections.m_rayToPositions[numRays][1] = rayToWorld[1];
|
||||||
|
command->m_requestRaycastIntersections.m_rayToPositions[numRays][2] = rayToWorld[2];
|
||||||
|
command->m_requestRaycastIntersections.m_numRays++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo)
|
void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo)
|
||||||
{
|
{
|
||||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||||
|
|||||||
@@ -332,6 +332,9 @@ b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle phy
|
|||||||
double rayFromWorldY, double rayFromWorldZ,
|
double rayFromWorldY, double rayFromWorldZ,
|
||||||
double rayToWorldX, double rayToWorldY, double rayToWorldZ);
|
double rayToWorldX, double rayToWorldY, double rayToWorldZ);
|
||||||
|
|
||||||
|
b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient);
|
||||||
|
void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]);
|
||||||
|
|
||||||
void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo);
|
void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo);
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -2235,58 +2235,74 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
|||||||
case CMD_REQUEST_RAY_CAST_INTERSECTIONS:
|
case CMD_REQUEST_RAY_CAST_INTERSECTIONS:
|
||||||
{
|
{
|
||||||
BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS");
|
BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS");
|
||||||
|
|
||||||
btVector3 rayFromWorld(clientCmd.m_requestRaycastIntersections.m_rayFromPosition[0],
|
|
||||||
clientCmd.m_requestRaycastIntersections.m_rayFromPosition[1],
|
|
||||||
clientCmd.m_requestRaycastIntersections.m_rayFromPosition[2]);
|
|
||||||
btVector3 rayToWorld(clientCmd.m_requestRaycastIntersections.m_rayToPosition[0],
|
|
||||||
clientCmd.m_requestRaycastIntersections.m_rayToPosition[1],
|
|
||||||
clientCmd.m_requestRaycastIntersections.m_rayToPosition[2]);
|
|
||||||
btCollisionWorld::ClosestRayResultCallback rayResultCallback(rayFromWorld,rayToWorld);
|
|
||||||
m_data->m_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,rayResultCallback);
|
|
||||||
serverStatusOut.m_raycastHits.m_numRaycastHits = 0;
|
serverStatusOut.m_raycastHits.m_numRaycastHits = 0;
|
||||||
|
|
||||||
if (rayResultCallback.hasHit())
|
for (int ray=0;ray<clientCmd.m_requestRaycastIntersections.m_numRays;ray++)
|
||||||
{
|
{
|
||||||
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitFraction
|
btVector3 rayFromWorld(clientCmd.m_requestRaycastIntersections.m_rayFromPositions[ray][0],
|
||||||
= rayResultCallback.m_closestHitFraction;
|
clientCmd.m_requestRaycastIntersections.m_rayFromPositions[ray][1],
|
||||||
|
clientCmd.m_requestRaycastIntersections.m_rayFromPositions[ray][2]);
|
||||||
|
btVector3 rayToWorld(clientCmd.m_requestRaycastIntersections.m_rayToPositions[ray][0],
|
||||||
|
clientCmd.m_requestRaycastIntersections.m_rayToPositions[ray][1],
|
||||||
|
clientCmd.m_requestRaycastIntersections.m_rayToPositions[ray][2]);
|
||||||
|
|
||||||
int objectUniqueId = -1;
|
btCollisionWorld::ClosestRayResultCallback rayResultCallback(rayFromWorld,rayToWorld);
|
||||||
int linkIndex = -1;
|
m_data->m_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,rayResultCallback);
|
||||||
|
int rayHits = serverStatusOut.m_raycastHits.m_numRaycastHits;
|
||||||
|
|
||||||
const btRigidBody* body = btRigidBody::upcast(rayResultCallback.m_collisionObject);
|
if (rayResultCallback.hasHit())
|
||||||
if (body)
|
|
||||||
{
|
{
|
||||||
objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2();
|
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitFraction
|
||||||
|
= rayResultCallback.m_closestHitFraction;
|
||||||
|
|
||||||
|
int objectUniqueId = -1;
|
||||||
|
int linkIndex = -1;
|
||||||
|
|
||||||
|
const btRigidBody* body = btRigidBody::upcast(rayResultCallback.m_collisionObject);
|
||||||
|
if (body)
|
||||||
|
{
|
||||||
|
objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2();
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(rayResultCallback.m_collisionObject);
|
||||||
|
if (mblB && mblB->m_multiBody)
|
||||||
|
{
|
||||||
|
linkIndex = mblB->m_link;
|
||||||
|
objectUniqueId = mblB->m_multiBody->getUserIndex2();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitObjectUniqueId
|
||||||
|
= objectUniqueId;
|
||||||
|
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitObjectLinkIndex
|
||||||
|
= linkIndex;
|
||||||
|
|
||||||
|
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[0]
|
||||||
|
= rayResultCallback.m_hitPointWorld[0];
|
||||||
|
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[1]
|
||||||
|
= rayResultCallback.m_hitPointWorld[1];
|
||||||
|
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[2]
|
||||||
|
= rayResultCallback.m_hitPointWorld[2];
|
||||||
|
|
||||||
|
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[0]
|
||||||
|
= rayResultCallback.m_hitNormalWorld[0];
|
||||||
|
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[1]
|
||||||
|
= rayResultCallback.m_hitNormalWorld[1];
|
||||||
|
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[2]
|
||||||
|
= rayResultCallback.m_hitNormalWorld[2];
|
||||||
|
|
||||||
} else
|
} else
|
||||||
{
|
{
|
||||||
const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(rayResultCallback.m_collisionObject);
|
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitFraction = 1;
|
||||||
if (mblB && mblB->m_multiBody)
|
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectUniqueId = -1;
|
||||||
{
|
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectLinkIndex = -1;
|
||||||
linkIndex = mblB->m_link;
|
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[0] = 0;
|
||||||
objectUniqueId = mblB->m_multiBody->getUserIndex2();
|
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[1] = 0;
|
||||||
}
|
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[2] = 0;
|
||||||
|
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[0] = 0;
|
||||||
|
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[1] = 0;
|
||||||
|
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[2] = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectUniqueId
|
|
||||||
= objectUniqueId;
|
|
||||||
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectLinkIndex
|
|
||||||
= linkIndex;
|
|
||||||
|
|
||||||
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[0]
|
|
||||||
= rayResultCallback.m_hitPointWorld[0];
|
|
||||||
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[1]
|
|
||||||
= rayResultCallback.m_hitPointWorld[1];
|
|
||||||
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[2]
|
|
||||||
= rayResultCallback.m_hitPointWorld[2];
|
|
||||||
|
|
||||||
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[0]
|
|
||||||
= rayResultCallback.m_hitNormalWorld[0];
|
|
||||||
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[1]
|
|
||||||
= rayResultCallback.m_hitNormalWorld[1];
|
|
||||||
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[2]
|
|
||||||
= rayResultCallback.m_hitNormalWorld[2];
|
|
||||||
|
|
||||||
serverStatusOut.m_raycastHits.m_numRaycastHits++;
|
serverStatusOut.m_raycastHits.m_numRaycastHits++;
|
||||||
}
|
}
|
||||||
serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED;
|
serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED;
|
||||||
|
|||||||
@@ -185,14 +185,15 @@ enum EnumRequestContactDataUpdateFlags
|
|||||||
|
|
||||||
struct RequestRaycastIntersections
|
struct RequestRaycastIntersections
|
||||||
{
|
{
|
||||||
double m_rayFromPosition[3];
|
int m_numRays;
|
||||||
double m_rayToPosition[3];
|
double m_rayFromPositions[MAX_RAY_INTERSECTION_BATCH_SIZE][3];
|
||||||
|
double m_rayToPositions[MAX_RAY_INTERSECTION_BATCH_SIZE][3];
|
||||||
};
|
};
|
||||||
|
|
||||||
struct SendRaycastHits
|
struct SendRaycastHits
|
||||||
{
|
{
|
||||||
int m_numRaycastHits;
|
int m_numRaycastHits;
|
||||||
b3RayHitInfo m_rayHits[MAX_RAY_HITS];
|
b3RayHitInfo m_rayHits[MAX_RAY_INTERSECTION_BATCH_SIZE];
|
||||||
};
|
};
|
||||||
|
|
||||||
struct RequestContactDataArgs
|
struct RequestContactDataArgs
|
||||||
|
|||||||
@@ -257,9 +257,13 @@ enum b3VREventType
|
|||||||
|
|
||||||
#define MAX_VR_BUTTONS 64
|
#define MAX_VR_BUTTONS 64
|
||||||
#define MAX_VR_CONTROLLERS 8
|
#define MAX_VR_CONTROLLERS 8
|
||||||
#define MAX_RAY_HITS 128
|
|
||||||
|
#define MAX_RAY_INTERSECTION_BATCH_SIZE 1024
|
||||||
|
#define MAX_RAY_HITS MAX_RAY_INTERSECTION_BATCH_SIZE
|
||||||
#define MAX_KEYBOARD_EVENTS 256
|
#define MAX_KEYBOARD_EVENTS 256
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
enum b3VRButtonInfo
|
enum b3VRButtonInfo
|
||||||
{
|
{
|
||||||
eButtonIsDown = 1,
|
eButtonIsDown = 1,
|
||||||
|
|||||||
@@ -400,8 +400,8 @@ static PyObject* pybullet_disconnectPhysicsServer(PyObject* self,
|
|||||||
return Py_None;
|
return Py_None;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
///to avoid memory leaks, disconnect all physics servers explicitly
|
||||||
void b3pybulletExitFunc()
|
void b3pybulletExitFunc(void)
|
||||||
{
|
{
|
||||||
int i;
|
int i;
|
||||||
for (i=0;i<MAX_PHYSICS_CLIENTS;i++)
|
for (i=0;i<MAX_PHYSICS_CLIENTS;i++)
|
||||||
@@ -2791,7 +2791,7 @@ static PyObject* pybullet_setTimeOut(PyObject* self, PyObject* args, PyObject* k
|
|||||||
return Py_None;
|
return Py_None;
|
||||||
}
|
}
|
||||||
|
|
||||||
static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject* keywds)
|
static PyObject* pybullet_rayTestObsolete(PyObject* self, PyObject* args, PyObject* keywds)
|
||||||
{
|
{
|
||||||
b3SharedMemoryCommandHandle commandHandle;
|
b3SharedMemoryCommandHandle commandHandle;
|
||||||
b3SharedMemoryStatusHandle statusHandle;
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
@@ -2875,6 +2875,154 @@ static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject* keyw
|
|||||||
return Py_None;
|
return Py_None;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* keywds)
|
||||||
|
{
|
||||||
|
b3SharedMemoryCommandHandle commandHandle;
|
||||||
|
b3SharedMemoryStatusHandle statusHandle;
|
||||||
|
int statusType;
|
||||||
|
PyObject* rayFromObjList = 0;
|
||||||
|
PyObject* rayToObjList = 0;
|
||||||
|
b3PhysicsClientHandle sm = 0;
|
||||||
|
int sizeFrom = 0;
|
||||||
|
int sizeTo = 0;
|
||||||
|
|
||||||
|
|
||||||
|
static char* kwlist[] = {"rayFromPositions", "rayToPositions", "physicsClientId", NULL};
|
||||||
|
int physicsClientId = 0;
|
||||||
|
|
||||||
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist,
|
||||||
|
&rayFromObjList, &rayToObjList, &physicsClientId))
|
||||||
|
return NULL;
|
||||||
|
|
||||||
|
sm = getPhysicsClient(physicsClientId);
|
||||||
|
if (sm == 0)
|
||||||
|
{
|
||||||
|
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
commandHandle = b3CreateRaycastBatchCommandInit(sm);
|
||||||
|
|
||||||
|
|
||||||
|
if (rayFromObjList)
|
||||||
|
{
|
||||||
|
PyObject* seqRayFromObj = PySequence_Fast(rayFromObjList, "expected a sequence of rayFrom positions");
|
||||||
|
PyObject* seqRayToObj = PySequence_Fast(rayToObjList, "expected a sequence of 'rayTo' positions");
|
||||||
|
|
||||||
|
if (seqRayFromObj && seqRayToObj)
|
||||||
|
{
|
||||||
|
int lenFrom = PySequence_Size(rayFromObjList);
|
||||||
|
int lenTo= PySequence_Size(seqRayToObj);
|
||||||
|
if (lenFrom!=lenTo)
|
||||||
|
{
|
||||||
|
PyErr_SetString(SpamError, "Size of from_positions need to be equal to size of to_positions.");
|
||||||
|
Py_DECREF(seqRayFromObj);
|
||||||
|
Py_DECREF(seqRayToObj);
|
||||||
|
return NULL;
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
int i;
|
||||||
|
|
||||||
|
if (lenFrom >= MAX_RAY_INTERSECTION_BATCH_SIZE)
|
||||||
|
{
|
||||||
|
PyErr_SetString(SpamError, "Number of rays exceed the maximum batch size.");
|
||||||
|
Py_DECREF(seqRayFromObj);
|
||||||
|
Py_DECREF(seqRayToObj);
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
for (i = 0; i < lenFrom; i++)
|
||||||
|
{
|
||||||
|
PyObject* rayFromObj = PySequence_GetItem(rayFromObjList,i);
|
||||||
|
PyObject* rayToObj = PySequence_GetItem(seqRayToObj,i);
|
||||||
|
double rayFromWorld[3];
|
||||||
|
double rayToWorld[3];
|
||||||
|
|
||||||
|
if ((pybullet_internalSetVectord(rayFromObj, rayFromWorld)) &&
|
||||||
|
(pybullet_internalSetVectord(rayToObj, rayToWorld)))
|
||||||
|
{
|
||||||
|
b3RaycastBatchAddRay(commandHandle, rayFromWorld, rayToWorld);
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
PyErr_SetString(SpamError, "Items in the from/to positions need to be an [x,y,z] list of 3 floats/doubles");
|
||||||
|
Py_DECREF(seqRayFromObj);
|
||||||
|
Py_DECREF(seqRayToObj);
|
||||||
|
Py_DECREF(rayFromObj);
|
||||||
|
Py_DECREF(rayToObj);
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
Py_DECREF(rayFromObj);
|
||||||
|
Py_DECREF(rayToObj);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
|
||||||
|
}
|
||||||
|
if (seqRayFromObj)
|
||||||
|
{
|
||||||
|
Py_DECREF(seqRayFromObj);
|
||||||
|
}
|
||||||
|
if (seqRayToObj)
|
||||||
|
{
|
||||||
|
Py_DECREF(seqRayToObj);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||||
|
statusType = b3GetStatusType(statusHandle);
|
||||||
|
if (statusType == CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED)
|
||||||
|
{
|
||||||
|
struct b3RaycastInformation raycastInfo;
|
||||||
|
PyObject* rayHitsObj = 0;
|
||||||
|
int i;
|
||||||
|
b3GetRaycastInformation(sm, &raycastInfo);
|
||||||
|
|
||||||
|
rayHitsObj = PyTuple_New(raycastInfo.m_numRayHits);
|
||||||
|
for (i = 0; i < raycastInfo.m_numRayHits; i++)
|
||||||
|
{
|
||||||
|
PyObject* singleHitObj = PyTuple_New(5);
|
||||||
|
{
|
||||||
|
PyObject* ob = PyInt_FromLong(raycastInfo.m_rayHits[i].m_hitObjectUniqueId);
|
||||||
|
PyTuple_SetItem(singleHitObj, 0, ob);
|
||||||
|
}
|
||||||
|
{
|
||||||
|
PyObject* ob = PyInt_FromLong(raycastInfo.m_rayHits[i].m_hitObjectLinkIndex);
|
||||||
|
PyTuple_SetItem(singleHitObj, 1, ob);
|
||||||
|
}
|
||||||
|
{
|
||||||
|
PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitFraction);
|
||||||
|
PyTuple_SetItem(singleHitObj, 2, ob);
|
||||||
|
}
|
||||||
|
{
|
||||||
|
PyObject* posObj = PyTuple_New(3);
|
||||||
|
int p;
|
||||||
|
for (p = 0; p < 3; p++)
|
||||||
|
{
|
||||||
|
PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitPositionWorld[p]);
|
||||||
|
PyTuple_SetItem(posObj, p, ob);
|
||||||
|
}
|
||||||
|
PyTuple_SetItem(singleHitObj, 3, posObj);
|
||||||
|
}
|
||||||
|
{
|
||||||
|
PyObject* normalObj = PyTuple_New(3);
|
||||||
|
int p;
|
||||||
|
for (p = 0; p < 3; p++)
|
||||||
|
{
|
||||||
|
PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitNormalWorld[p]);
|
||||||
|
PyTuple_SetItem(normalObj, p, ob);
|
||||||
|
}
|
||||||
|
PyTuple_SetItem(singleHitObj, 4, normalObj);
|
||||||
|
}
|
||||||
|
PyTuple_SetItem(rayHitsObj, i, singleHitObj);
|
||||||
|
}
|
||||||
|
return rayHitsObj;
|
||||||
|
}
|
||||||
|
|
||||||
|
Py_INCREF(Py_None);
|
||||||
|
return Py_None;
|
||||||
|
}
|
||||||
|
|
||||||
static PyObject* pybullet_getMatrixFromQuaternion(PyObject* self, PyObject* args)
|
static PyObject* pybullet_getMatrixFromQuaternion(PyObject* self, PyObject* args)
|
||||||
{
|
{
|
||||||
PyObject* quatObj;
|
PyObject* quatObj;
|
||||||
@@ -5298,9 +5446,14 @@ static PyMethodDef SpamMethods[] = {
|
|||||||
"fileName, optional objectUniqueId. Function returns int loggingUniqueId"},
|
"fileName, optional objectUniqueId. Function returns int loggingUniqueId"},
|
||||||
{"stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS,
|
{"stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS,
|
||||||
"Stop logging of robot state, given a loggingUniqueId."},
|
"Stop logging of robot state, given a loggingUniqueId."},
|
||||||
{"rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS,
|
{"rayTest", (PyCFunction)pybullet_rayTestObsolete, METH_VARARGS | METH_KEYWORDS,
|
||||||
"Cast a ray and return the first object hit, if any. "
|
"Cast a ray and return the first object hit, if any. "
|
||||||
"Takes two arguments (from position [x,y,z] and to position [x,y,z] in Cartesian world coordinates"},
|
"Takes two arguments (from_position [x,y,z] and to_position [x,y,z] in Cartesian world coordinates"},
|
||||||
|
|
||||||
|
{"rayTestBatch", (PyCFunction)pybullet_rayTestBatch, METH_VARARGS | METH_KEYWORDS,
|
||||||
|
"Cast a batch of rays and return the result for each of the rays (first object hit, if any. or -1) "
|
||||||
|
"Takes two arguments (list of from_positions [x,y,z] and a list of to_positions [x,y,z] in Cartesian world coordinates"},
|
||||||
|
|
||||||
{"setTimeOut", (PyCFunction)pybullet_setTimeOut, METH_VARARGS | METH_KEYWORDS,
|
{"setTimeOut", (PyCFunction)pybullet_setTimeOut, METH_VARARGS | METH_KEYWORDS,
|
||||||
"Set the timeOut in seconds, used for most of the API calls."},
|
"Set the timeOut in seconds, used for most of the API calls."},
|
||||||
// todo(erwincoumans)
|
// todo(erwincoumans)
|
||||||
@@ -5436,7 +5589,9 @@ initpybullet(void)
|
|||||||
|
|
||||||
PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE);
|
PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE);
|
||||||
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION);
|
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION);
|
||||||
|
|
||||||
|
PyModule_AddIntConstant(m, "MAX_RAY_INTERSECTION_BATCH_SIZE", MAX_RAY_INTERSECTION_BATCH_SIZE);
|
||||||
|
|
||||||
PyModule_AddIntConstant(m, "B3G_F1", B3G_F1);
|
PyModule_AddIntConstant(m, "B3G_F1", B3G_F1);
|
||||||
PyModule_AddIntConstant(m, "B3G_F2", B3G_F2);
|
PyModule_AddIntConstant(m, "B3G_F2", B3G_F2);
|
||||||
PyModule_AddIntConstant(m, "B3G_F3", B3G_F3);
|
PyModule_AddIntConstant(m, "B3G_F3", B3G_F3);
|
||||||
|
|||||||
Reference in New Issue
Block a user