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

pybullet getOverlappingObjects added: report all object unique ids within a world space bounding box (aabb)

Browse Source
This commit is contained in:
erwincoumans
2016-11-10 11:22:22 -08:00
parent b16d9abddd
commit 9b5aa9a723
12 changed files with 299 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/SharedMemory/PhysicsClient.h
View File

@@ -48,6 +48,8 @@ public:
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData)=0;
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects) = 0;
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) = 0;
};

35
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1310,28 +1310,31 @@ void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, do
///get all the bodies that touch a given axis aligned bounding box specified in world space (min and max coordinates)
b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[3], const double aabbMax[3])
{
b3SharedMemoryCommandHandle commandHandle = b3InitRequestContactPointInformation(physClient);
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags = CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE;
command->m_requestContactPointArguments.m_mode = CONTACT_QUERY_MODE_AABB_OVERLAP;
command->m_type = CMD_REQUEST_AABB_OVERLAP;
command->m_updateFlags = 0;
command->m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = 0;
command->m_requestOverlappingObjectsArgs.m_aabbQueryMin[0] = aabbMin[0];
command->m_requestOverlappingObjectsArgs.m_aabbQueryMin[1] = aabbMin[1];
command->m_requestOverlappingObjectsArgs.m_aabbQueryMin[2] = aabbMin[2];
command->m_requestContactPointArguments.m_aabbQueryMin[0] = aabbMin[0];
command->m_requestContactPointArguments.m_aabbQueryMin[1] = aabbMin[1];
command->m_requestContactPointArguments.m_aabbQueryMin[2] = aabbMin[2];
command->m_requestContactPointArguments.m_aabbQueryMax[0] = aabbMax[0];
command->m_requestContactPointArguments.m_aabbQueryMax[1] = aabbMax[1];
command->m_requestContactPointArguments.m_aabbQueryMax[2] = aabbMax[2];
return commandHandle;
command->m_requestOverlappingObjectsArgs.m_aabbQueryMax[0] = aabbMax[0];
command->m_requestOverlappingObjectsArgs.m_aabbQueryMax[1] = aabbMax[1];
command->m_requestOverlappingObjectsArgs.m_aabbQueryMax[2] = aabbMax[2];
return (b3SharedMemoryCommandHandle)command;
}
void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data)
{
data->m_numOverlappingObjects = 0;
// data->m_objectUniqueIds
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
cl->getCachedOverlappingObjects(data);
}
}

46
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -40,7 +40,7 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<int> m_cachedSegmentationMaskBuffer;
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
btAlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
@@ -596,6 +596,30 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("Inverse Dynamics computations failed");
break;
}
case CMD_REQUEST_AABB_OVERLAP_FAILED:
{
b3Warning("Overlapping object query failed");
break;
}
case CMD_REQUEST_AABB_OVERLAP_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Overlapping object request completed");
}
int startOverlapIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
int numOverlapCopied = serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied;
m_data->m_cachedOverlappingObjects.resize(startOverlapIndex + numOverlapCopied);
b3OverlappingObject* objects = (b3OverlappingObject*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
for (int i = 0; i < numOverlapCopied; i++)
{
m_data->m_cachedOverlappingObjects[startOverlapIndex + i] = objects[i];
}
break;
}
case CMD_CONTACT_POINT_INFORMATION_COMPLETED:
{
if (m_data->m_verboseOutput)
@@ -741,6 +765,18 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
}
}
if (serverCmd.m_type == CMD_REQUEST_AABB_OVERLAP_COMPLETED)
{
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
if (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied)
{
command.m_type = CMD_REQUEST_AABB_OVERLAP;
command.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex + serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied;
submitClientCommand(command);
return 0;
}
}
if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
{
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
@@ -873,6 +909,14 @@ void PhysicsClientSharedMemory::getCachedContactPointInformation(struct b3Contac
}
void PhysicsClientSharedMemory::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects)
{
overlappingObjects->m_numOverlappingObjects = m_data->m_cachedOverlappingObjects.size();
overlappingObjects->m_overlappingObjects = m_data->m_cachedOverlappingObjects.size() ?
&m_data->m_cachedOverlappingObjects[0] : 0;
}
void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
{
visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();

2
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -57,6 +57,8 @@ public:
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
};

68
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -44,6 +44,7 @@ struct PhysicsDirectInternalData
btAlignedObjectArray<int> m_cachedSegmentationMask;
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
btAlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
@@ -309,6 +310,60 @@ bool PhysicsDirect::processVisualShapeData(const struct SharedMemoryCommand& org
return m_data->m_hasStatus;
}
bool PhysicsDirect::processOverlappingObjects(const struct SharedMemoryCommand& orgCommand)
{
SharedMemoryCommand command = orgCommand;
const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
do
{
bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
int timeout = 1024 * 1024 * 1024;
while ((!hasStatus) && (timeout-- > 0))
{
const SharedMemoryStatus* stat = processServerStatus();
if (stat)
{
hasStatus = true;
}
}
m_data->m_hasStatus = hasStatus;
if (hasStatus)
{
if (m_data->m_verboseOutput)
{
b3Printf("Overlapping Objects Request OK\n");
}
int startOverlapIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
int numOverlapCopied = serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied;
m_data->m_cachedOverlappingObjects.resize(startOverlapIndex + numOverlapCopied);
b3OverlappingObject* objects = (b3OverlappingObject*)&m_data->m_bulletStreamDataServerToClient[0];
for (int i = 0; i < numOverlapCopied; i++)
{
m_data->m_cachedOverlappingObjects[startOverlapIndex + i] = objects[i];
}
if (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied)
{
m_data->m_hasStatus = false;
command.m_type = CMD_REQUEST_AABB_OVERLAP;
command.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex + serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied;
}
}
} while (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied);
return m_data->m_hasStatus;
}
bool PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& orgCommand)
{
@@ -525,6 +580,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
{
switch (serverCmd.m_type)
{
case CMD_REQUEST_INTERNAL_DATA_COMPLETED:
{
if (serverCmd.m_numDataStreamBytes)
@@ -635,6 +691,10 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
{
return processVisualShapeData(command);
}
if (command.m_type == CMD_REQUEST_AABB_OVERLAP)
{
return processOverlappingObjects(command);
}
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
@@ -761,6 +821,14 @@ void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation
}
void PhysicsDirect::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects)
{
overlappingObjects->m_numOverlappingObjects = m_data->m_cachedOverlappingObjects.size();
overlappingObjects->m_overlappingObjects = m_data->m_cachedOverlappingObjects.size() ?
&m_data->m_cachedOverlappingObjects[0] : 0;
}
void PhysicsDirect::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
{
visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();

5
examples/SharedMemory/PhysicsDirect.h
View File

@@ -20,6 +20,8 @@ protected:
bool processContactPointData(const struct SharedMemoryCommand& orgCommand);
bool processOverlappingObjects(const struct SharedMemoryCommand& orgCommand);
bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
@@ -75,8 +77,11 @@ public:
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
//those 2 APIs are for internal use for visualization
virtual bool connect(struct GUIHelperInterface* guiHelper);
virtual void renderScene();

6
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -145,3 +145,9 @@ void PhysicsLoopBack::getCachedVisualShapeInformation(struct b3VisualShapeInform
{
return m_data->m_physicsClient->getCachedVisualShapeInformation(visualShapesInfo);
}
void PhysicsLoopBack::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects)
{
return m_data->m_physicsClient->getCachedOverlappingObjects(overlappingObjects);
}

2
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -62,6 +62,8 @@ public:
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
};

166
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -318,6 +318,45 @@ struct SaveWorldObjectData
std::string m_fileName;
};
struct MyBroadphaseCallback : public btBroadphaseAabbCallback
{
b3AlignedObjectArray<int> m_bodyUniqueIds;
b3AlignedObjectArray<int> m_links;
MyBroadphaseCallback()
{
}
virtual ~MyBroadphaseCallback()
{
}
void clear()
{
m_bodyUniqueIds.clear();
m_links.clear();
}
virtual bool process(const btBroadphaseProxy* proxy)
{
btCollisionObject* colObj = (btCollisionObject*)proxy->m_clientObject;
btMultiBodyLinkCollider* mbl = btMultiBodyLinkCollider::upcast(colObj);
if (mbl)
{
int bodyUniqueId = mbl->m_multiBody->getUserIndex2();
m_bodyUniqueIds.push_back(bodyUniqueId);
m_links.push_back(mbl->m_link);
return true;
}
int bodyUniqueId = colObj->getUserIndex2();
if (bodyUniqueId >= 0)
{
m_bodyUniqueIds.push_back(bodyUniqueId);
m_links.push_back(mbl->m_link);
}
return true;
}
};
struct PhysicsServerCommandProcessorInternalData
{
///handle management
@@ -453,6 +492,8 @@ struct PhysicsServerCommandProcessorInternalData
SharedMemoryDebugDrawer* m_remoteDebugDrawer;
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
MyBroadphaseCallback m_cachedOverlappingObjects;
btAlignedObjectArray<int> m_sdfRecentLoadedBodies;
@@ -2535,6 +2576,62 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = true;
break;
}
case CMD_REQUEST_AABB_OVERLAP:
{
SharedMemoryStatus& serverCmd = serverStatusOut;
int curObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
if (0== curObjectIndex)
{
//clientCmd.m_requestContactPointArguments.m_aabbQueryMin
btVector3 aabbMin, aabbMax;
aabbMin.setValue(clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[0],
clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[1],
clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[2]);
aabbMax.setValue(clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[0],
clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[1],
clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[2]);
m_data->m_cachedOverlappingObjects.clear();
m_data->m_dynamicsWorld->getBroadphase()->aabbTest(aabbMin, aabbMax, m_data->m_cachedOverlappingObjects);
}
int totalBytesPerObject = sizeof(b3OverlappingObject);
int overlapCapacity = bufferSizeInBytes / totalBytesPerObject - 1;
int numOverlap = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size();
int remainingObjects = numOverlap - curObjectIndex;
int curNumObjects = btMin(overlapCapacity, remainingObjects);
if (numOverlap < overlapCapacity)
{
b3OverlappingObject* overlapStorage = (b3OverlappingObject*)bufferServerToClient;
for (int i = 0; i < m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size(); i++)
{
overlapStorage[i].m_objectUniqueId = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds[i];
overlapStorage[i].m_linkIndex = m_data->m_cachedOverlappingObjects.m_links[i];
}
serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_COMPLETED;
int m_startingOverlappingObjectIndex;
int m_numOverlappingObjectsCopied;
int m_numRemainingOverlappingObjects;
serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size();
serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects = remainingObjects - curNumObjects;
}
else
{
serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_FAILED;
}
hasStatus = true;
break;
}
case CMD_REQUEST_CONTACT_POINT_INFORMATION:
{
SharedMemoryStatus& serverCmd =serverStatusOut;
@@ -2634,74 +2731,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
break;
}
case CONTACT_QUERY_MODE_AABB_OVERLAP:
{
//clientCmd.m_requestContactPointArguments.m_aabbQueryMin
btVector3 aabbMin,aabbMax;
aabbMin.setValue(clientCmd.m_requestContactPointArguments.m_aabbQueryMin[0],
clientCmd.m_requestContactPointArguments.m_aabbQueryMin[1],
clientCmd.m_requestContactPointArguments.m_aabbQueryMin[2]);
aabbMax.setValue(clientCmd.m_requestContactPointArguments.m_aabbQueryMax[0],
clientCmd.m_requestContactPointArguments.m_aabbQueryMax[1],
clientCmd.m_requestContactPointArguments.m_aabbQueryMax[2]);
struct MyBroadphaseCallback : public btBroadphaseAabbCallback
{
b3AlignedObjectArray<int> m_bodyUniqueIds;
b3AlignedObjectArray<int> m_links;
MyBroadphaseCallback()
{
}
virtual ~MyBroadphaseCallback()
{
}
virtual bool process(const btBroadphaseProxy* proxy)
{
btCollisionObject* colObj = (btCollisionObject*)proxy->m_clientObject;
btMultiBodyLinkCollider* mbl = btMultiBodyLinkCollider::upcast(colObj);
if (mbl)
{
int bodyUniqueId = mbl->m_multiBody->getUserIndex2();
m_bodyUniqueIds.push_back(bodyUniqueId);
m_links.push_back(mbl->m_link);
return true;
}
int bodyUniqueId = colObj->getUserIndex2();
if (bodyUniqueId >= 0)
{
m_bodyUniqueIds.push_back(bodyUniqueId);
m_links.push_back(mbl->m_link);
}
return true;
}
};
MyBroadphaseCallback callback;
m_data->m_dynamicsWorld->getBroadphase()->aabbTest(aabbMin,aabbMax,callback);
int totalBytesPerObject = 2 * sizeof(int);
int pairCapacity = bufferSizeInBytes / totalBytesPerObject - 1;
if (callback.m_bodyUniqueIds.size() < pairCapacity)
{
serverCmd.m_type = CMD_AABB_OVERLAP_COMPLETED;
int* pairStorage = (int*)bufferServerToClient;
for (int i = 0; i < callback.m_bodyUniqueIds.size(); i++)
{
pairStorage[i * 2] = callback.m_bodyUniqueIds[i];
pairStorage[i * 2+1] = callback.m_links[i];
}
}
else
{
serverCmd.m_type = CMD_AABB_OVERLAP_FAILED;
}
hasStatus = true;
break;
}
case CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS:
{
//todo(erwincoumans) compute closest points between all, and vs all, pair
@@ -3863,7 +3893,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
}
else
{
motor->setPositionTarget(posTarget - correction*5., 1);
motor->setPositionTarget(posTarget, 1);
}
motor->setVelocityTarget(0, 0.5);
btScalar maxImp = (1+0.1*i)*m_data->m_physicsDeltaTime;

16
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -155,9 +155,14 @@ struct RequestContactDataArgs
int m_objectAIndexFilter;
int m_objectBIndexFilter;
double m_closestDistanceThreshold;
int m_mode;
};
struct RequestOverlappingObjectsArgs
{
int m_startingOverlappingObjectIndex;
double m_aabbQueryMin[3];
double m_aabbQueryMax[3];
int m_mode;
};
struct RequestVisualShapeDataArgs
@@ -520,6 +525,7 @@ struct SharedMemoryCommand
struct CalculateJacobianArgs m_calculateJacobianArguments;
struct CreateJointArgs m_createJointArguments;
struct RequestContactDataArgs m_requestContactPointArguments;
struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs;
struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments;
struct UpdateVisualShapeDataArgs m_updateVisualShapeDataArguments;
struct LoadTextureArgs m_loadTextureArguments;
@@ -540,6 +546,13 @@ struct SendContactDataArgs
int m_numRemainingContactPoints;
};
struct SendOverlappingObjectsArgs
{
int m_startingOverlappingObjectIndex;
int m_numOverlappingObjectsCopied;
int m_numRemainingOverlappingObjects;
};
struct SharedMemoryStatus
{
int m_type;
@@ -562,6 +575,7 @@ struct SharedMemoryStatus
struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
struct CalculateJacobianResultArgs m_jacobianResultArgs;
struct SendContactDataArgs m_sendContactPointArgs;
struct SendOverlappingObjectsArgs m_sendOverlappingObjectsArgs;
struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
};

15
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -34,6 +34,7 @@ enum EnumSharedMemoryClientCommand
CMD_CALCULATE_JACOBIAN,
CMD_CREATE_JOINT,
CMD_REQUEST_CONTACT_POINT_INFORMATION,
CMD_REQUEST_AABB_OVERLAP,
CMD_SAVE_WORLD,
CMD_REQUEST_VISUAL_SHAPE_INFO,
CMD_UPDATE_VISUAL_SHAPE,
@@ -80,8 +81,8 @@ enum EnumSharedMemoryServerStatus
CMD_CALCULATED_JACOBIAN_FAILED,
CMD_CONTACT_POINT_INFORMATION_COMPLETED,
CMD_CONTACT_POINT_INFORMATION_FAILED,
CMD_AABB_OVERLAP_COMPLETED,
CMD_AABB_OVERLAP_FAILED,
CMD_REQUEST_AABB_OVERLAP_COMPLETED,
CMD_REQUEST_AABB_OVERLAP_FAILED,
CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
CMD_SAVE_WORLD_COMPLETED,
@@ -160,11 +161,16 @@ struct b3DebugLines
const float* m_linesColor;//float red,green,blue times 'm_numDebugLines'.
};
struct b3OverlappingObject
{
int m_objectUniqueId;
int m_linkIndex;
};
struct b3AABBOverlapData
{
int m_numOverlappingObjects;
int* m_objectUniqueIds;
int* m_links;
struct b3OverlappingObject* m_overlappingObjects;
};
struct b3CameraImageData
@@ -205,7 +211,6 @@ enum
{
CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS = 0,
CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS = 1,
CONTACT_QUERY_MODE_AABB_OVERLAP = 2,
};

27
examples/pybullet/pybullet.c
View File

@@ -682,8 +682,7 @@ static int pybullet_internalGetBasePositionAndOrientation(
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
Py_INCREF(Py_None);
return Py_None;
return 0;
}
{
@@ -1628,6 +1627,7 @@ static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args,
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
struct b3AABBOverlapData overlapData;
int i;
static char *kwlist[] = { "aabbMin", "aabbMax", NULL };
if (0 == sm) {
@@ -1646,6 +1646,27 @@ static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args,
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
b3GetAABBOverlapResults(sm, &overlapData);
if (overlapData.m_numOverlappingObjects)
{
PyObject* pyResultList = PyTuple_New(overlapData.m_numOverlappingObjects);
//For huge amount of overlap, we could use numpy instead (see camera pixel data)
//What would Python do with huge amount of data? Pass it onto TensorFlow!
for (i = 0; i < overlapData.m_numOverlappingObjects; i++) {
PyObject* overlap = PyTuple_New(2);//body unique id and link index
PyObject* item;
item =
PyInt_FromLong(overlapData.m_overlappingObjects[i].m_objectUniqueId);
PyTuple_SetItem(overlap, 0, item);
item =
PyInt_FromLong(overlapData.m_overlappingObjects[i].m_linkIndex);
PyTuple_SetItem(overlap, 1, item);
PyTuple_SetItem(pyResultList, i, overlap);
}
return pyResultList;
}
Py_INCREF(Py_None);
return Py_None;
@@ -1700,7 +1721,6 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
}
static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, PyObject *keywds) {
int size = PySequence_Size(args);
int bodyUniqueIdA = -1;
@@ -1710,7 +1730,6 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py
struct b3ContactInformation contactPointData;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int i;
PyObject* pyResultList = 0;