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

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

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);
+	PhysicsClient* cl = (PhysicsClient*)physClient;
-	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
 	b3Assert(command);
-	b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
+	command->m_type = CMD_REQUEST_AABB_OVERLAP;
-	command->m_updateFlags = CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE;
+	command->m_updateFlags = 0;
-	command->m_requestContactPointArguments.m_mode = CONTACT_QUERY_MODE_AABB_OVERLAP;
+	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_requestOverlappingObjectsArgs.m_aabbQueryMax[0] = aabbMax[0];
-	command->m_requestContactPointArguments.m_aabbQueryMin[1] = aabbMin[1];
+	command->m_requestOverlappingObjectsArgs.m_aabbQueryMax[1] = aabbMax[1];
-	command->m_requestContactPointArguments.m_aabbQueryMin[2] = aabbMin[2];
+	command->m_requestOverlappingObjectsArgs.m_aabbQueryMax[2] = aabbMax[2];
-
+	return  (b3SharedMemoryCommandHandle)command;
-	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;
 }
 
 void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data)
 {
-	data->m_numOverlappingObjects = 0;
+	PhysicsClient* cl = (PhysicsClient*)physClient;
-//	data->m_objectUniqueIds
+	if (cl)
+	{
+		cl->getCachedOverlappingObjects(data);
+	}
 }
 
 

examples/SharedMemory/PhysicsClientSharedMemory.cpp

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

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

examples/SharedMemory/PhysicsDirect.cpp

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

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

examples/SharedMemory/PhysicsLoopBack.cpp

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

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

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

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

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

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_REQUEST_AABB_OVERLAP_COMPLETED,
-		CMD_AABB_OVERLAP_FAILED,
+		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;
+	struct b3OverlappingObject* m_overlappingObjects;
-	int* m_links;
 };
 
 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,
 };
 
 

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 0;
-	  return Py_None;
   }
 
   {
@@ -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;