From 9b5aa9a72382a5e29c678b3570b1c590d503d896 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 10 Nov 2016 11:22:22 -0800 Subject: [PATCH] pybullet getOverlappingObjects added: report all object unique ids within a world space bounding box (aabb) --- examples/SharedMemory/PhysicsClient.h | 2 + examples/SharedMemory/PhysicsClientC_API.cpp | 35 ++-- .../PhysicsClientSharedMemory.cpp | 46 ++++- .../SharedMemory/PhysicsClientSharedMemory.h | 2 + examples/SharedMemory/PhysicsDirect.cpp | 70 +++++++- examples/SharedMemory/PhysicsDirect.h | 5 + examples/SharedMemory/PhysicsLoopBack.cpp | 8 +- examples/SharedMemory/PhysicsLoopBack.h | 2 + .../PhysicsServerCommandProcessor.cpp | 168 +++++++++++------- examples/SharedMemory/SharedMemoryCommands.h | 16 +- examples/SharedMemory/SharedMemoryPublic.h | 15 +- examples/pybullet/pybullet.c | 29 ++- 12 files changed, 299 insertions(+), 99 deletions(-) diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index f028f537f..e1c20cadb 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -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; }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 8dcfdebd1..f95ff129a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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); + } } diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 109b66b9c..0a3d198eb 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -40,7 +40,7 @@ struct PhysicsClientSharedMemoryInternalData { btAlignedObjectArray m_cachedSegmentationMaskBuffer; btAlignedObjectArray m_cachedContactPoints; - + btAlignedObjectArray m_cachedOverlappingObjects; btAlignedObjectArray m_cachedVisualShapes; btAlignedObjectArray 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) @@ -740,6 +764,18 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus; } } + + 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) { @@ -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(); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index 1ebe5bccd..d5f6b3cff 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -57,6 +57,8 @@ public: virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData); + virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects); + virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo); }; diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index db408e529..b54485f66 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -44,7 +44,8 @@ struct PhysicsDirectInternalData btAlignedObjectArray m_cachedSegmentationMask; btAlignedObjectArray m_cachedContactPoints; - + btAlignedObjectArray m_cachedOverlappingObjects; + btAlignedObjectArray m_cachedVisualShapes; PhysicsCommandProcessorInterface* m_commandProcessor; @@ -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(); diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 968f8f560..963281f7e 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -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(); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index f669dde30..3e9c3d77d 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -144,4 +144,10 @@ void PhysicsLoopBack::getCachedContactPointInformation(struct b3ContactInformati void PhysicsLoopBack::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) { return m_data->m_physicsClient->getCachedVisualShapeInformation(visualShapesInfo); -} \ No newline at end of file +} + +void PhysicsLoopBack::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects) +{ + return m_data->m_physicsClient->getCachedOverlappingObjects(overlappingObjects); +} + diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index 42acca718..c9187c4b8 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -62,6 +62,8 @@ public: virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData); + virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects); + virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo); }; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 5e3d94f72..28ed1a81d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -318,6 +318,45 @@ struct SaveWorldObjectData std::string m_fileName; }; +struct MyBroadphaseCallback : public btBroadphaseAabbCallback +{ + b3AlignedObjectArray m_bodyUniqueIds; + b3AlignedObjectArray 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 m_cachedContactPoints; + MyBroadphaseCallback m_cachedOverlappingObjects; + btAlignedObjectArray 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 m_bodyUniqueIds; - b3AlignedObjectArray 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; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 2ba38f232..bdc2dd96e 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -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; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 4f55d5520..7f8179b6c 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -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, }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 6124f2f57..a38d427fd 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -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,7 +1627,8 @@ 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) { PyErr_SetString(SpamError, "Not connected to physics server."); @@ -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;