Merge pull request #1894 from erwincoumans/master

allow to disable self-collision for a link-pair, only enable eglPlugin for Linux (disable eglPlugin for Win/Mac)

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -2858,6 +2858,22 @@ B3_SHARED_API	b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClien
 	return (b3SharedMemoryCommandHandle)command;
 }
 
+B3_SHARED_API	b3SharedMemoryCommandHandle b3InitRemoveCollisionShapeCommand(b3PhysicsClientHandle physClient, int collisionShapeId)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+    b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    
+    command->m_type = CMD_REMOVE_BODY;
+	command->m_updateFlags = 0;
+	command->m_removeObjectArgs.m_numBodies = 0;
+	command->m_removeObjectArgs.m_numUserConstraints = 0;
+	command->m_removeObjectArgs.m_numUserCollisionShapes = 1;
+	command->m_removeObjectArgs.m_userCollisionShapes[0] = collisionShapeId;
+	return (b3SharedMemoryCommandHandle)command;
+}
 B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle)
 {
 	const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
@@ -3883,10 +3899,76 @@ B3_SHARED_API void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle com
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
 	b3Assert(command);
 	b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
-	command->m_updateFlags += CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD;
+	command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD;
 	command->m_requestContactPointArguments.m_closestDistanceThreshold = distance;
 }
 
+B3_SHARED_API	void b3SetClosestDistanceFilterCollisionShapeA(b3SharedMemoryCommandHandle commandHandle, int collisionShapeA)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
+	command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_A;
+	command->m_requestContactPointArguments.m_collisionShapeA = collisionShapeA;
+}
+
+B3_SHARED_API	void b3SetClosestDistanceFilterCollisionShapeB(b3SharedMemoryCommandHandle commandHandle, int collisionShapeB)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
+	command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_B;
+	command->m_requestContactPointArguments.m_collisionShapeB = collisionShapeB;
+}
+
+B3_SHARED_API	void b3SetClosestDistanceFilterCollisionShapePositionA(b3SharedMemoryCommandHandle commandHandle, double collisionShapePositionA[/*3*/])
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
+	command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_A;
+	command->m_requestContactPointArguments.m_collisionShapePositionA[0] = collisionShapePositionA[0];
+	command->m_requestContactPointArguments.m_collisionShapePositionA[1] = collisionShapePositionA[1];
+	command->m_requestContactPointArguments.m_collisionShapePositionA[2] = collisionShapePositionA[2];
+}
+
+B3_SHARED_API	void b3SetClosestDistanceFilterCollisionShapePositionB(b3SharedMemoryCommandHandle commandHandle, double collisionShapePositionB[/*3*/])
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
+	command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_B;
+	command->m_requestContactPointArguments.m_collisionShapePositionB[0] = collisionShapePositionB[0];
+	command->m_requestContactPointArguments.m_collisionShapePositionB[1] = collisionShapePositionB[1];
+	command->m_requestContactPointArguments.m_collisionShapePositionB[2] = collisionShapePositionB[2];
+}
+
+B3_SHARED_API	void b3SetClosestDistanceFilterCollisionShapeOrientationA(b3SharedMemoryCommandHandle commandHandle, double collisionShapeOrientationA[/*4*/])
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
+	command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_A;
+	command->m_requestContactPointArguments.m_collisionShapeOrientationA[0] = collisionShapeOrientationA[0];
+	command->m_requestContactPointArguments.m_collisionShapeOrientationA[1] = collisionShapeOrientationA[1];
+	command->m_requestContactPointArguments.m_collisionShapeOrientationA[2] = collisionShapeOrientationA[2];
+	command->m_requestContactPointArguments.m_collisionShapeOrientationA[3] = collisionShapeOrientationA[3];
+}
+
+
+B3_SHARED_API	void b3SetClosestDistanceFilterCollisionShapeOrientationB(b3SharedMemoryCommandHandle commandHandle, double collisionShapeOrientationB[/*4*/])
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
+	command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_B;
+	command->m_requestContactPointArguments.m_collisionShapeOrientationB[0] = collisionShapeOrientationB[0];
+	command->m_requestContactPointArguments.m_collisionShapeOrientationB[1] = collisionShapeOrientationB[1];
+	command->m_requestContactPointArguments.m_collisionShapeOrientationB[2] = collisionShapeOrientationB[2];
+	command->m_requestContactPointArguments.m_collisionShapeOrientationB[3] = collisionShapeOrientationB[3];
+}
+
+
 
 ///get all the bodies that touch a given axis aligned bounding box specified in world space (min and max coordinates)
 B3_SHARED_API	b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[3], const double aabbMax[3])

examples/SharedMemory/PhysicsClientC_API.h

@@ -290,6 +290,15 @@ B3_SHARED_API	void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle c
 B3_SHARED_API	void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
 B3_SHARED_API	void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
 B3_SHARED_API	void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance);
+
+B3_SHARED_API	void b3SetClosestDistanceFilterCollisionShapeA(b3SharedMemoryCommandHandle commandHandle, int collisionShapeA);
+B3_SHARED_API	void b3SetClosestDistanceFilterCollisionShapeB(b3SharedMemoryCommandHandle commandHandle, int collisionShapeB);
+B3_SHARED_API	void b3SetClosestDistanceFilterCollisionShapePositionA(b3SharedMemoryCommandHandle commandHandle, double collisionShapePositionA[/*3*/]);
+B3_SHARED_API	void b3SetClosestDistanceFilterCollisionShapePositionB(b3SharedMemoryCommandHandle commandHandle, double collisionShapePositionB[/*3*/]);
+B3_SHARED_API	void b3SetClosestDistanceFilterCollisionShapeOrientationA(b3SharedMemoryCommandHandle commandHandle, double collisionShapeOrientationA[/*4*/]);
+B3_SHARED_API	void b3SetClosestDistanceFilterCollisionShapeOrientationB(b3SharedMemoryCommandHandle commandHandle, double collisionShapeOrientationB[/*4*/]);
+
+
 B3_SHARED_API	void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
 
 ///get all the bodies that touch a given axis aligned bounding box specified in world space (min and max coordinates)
@@ -487,6 +496,8 @@ B3_SHARED_API	void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandH
 B3_SHARED_API	void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[/*3*/], double childOrientation[/*4*/]);
 B3_SHARED_API	int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
 
+B3_SHARED_API	b3SharedMemoryCommandHandle b3InitRemoveCollisionShapeCommand(b3PhysicsClientHandle physClient, int collisionShapeId);
+
 B3_SHARED_API	b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient);
 B3_SHARED_API	int b3CreateVisualShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius);
 B3_SHARED_API	int b3CreateVisualShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[/*3*/]);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -232,9 +232,16 @@ struct InternalCollisionShapeData
 {
 	btCollisionShape* m_collisionShape;
 	b3AlignedObjectArray<UrdfCollision> m_urdfCollisionObjects;
+	int m_used;
+	InternalCollisionShapeData()
+		:m_collisionShape(0),
+		m_used(0)
+	{
+	}
 	void clear()
 	{
 		m_collisionShape=0;
+		m_used = 0;
 	}
 };
 
@@ -1693,6 +1700,7 @@ struct PhysicsServerCommandProcessorInternalData
 	b3HashMap<b3HashString, UrdfVisualShapeCache> m_cachedVUrdfisualShapes;
 
 	b3ThreadPool* m_threadPool;
+	btScalar m_defaultCollisionMargin;
 
 	PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc)
 		:m_pluginManager(proc),
@@ -1725,7 +1733,8 @@ struct PhysicsServerCommandProcessorInternalData
 		m_pdControlPlugin(-1),
 		m_collisionFilterPlugin(-1),
 		m_grpcPlugin(-1),
-		m_threadPool(0)
+		m_threadPool(0),
+		m_defaultCollisionMargin(0.001)
 	{
 
 		{
@@ -2381,6 +2390,7 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
 			InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId);
 			if (handle && handle->m_collisionShape)
 			{
+				handle->m_used++;
 				if (handle->m_collisionShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
 				{
 					btCompoundShape* childCompound = (btCompoundShape*)handle->m_collisionShape;
@@ -3965,8 +3975,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 {
 	bool hasStatus = true;
 	serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED;
-	btScalar defaultCollisionMargin = 0.001;
+	
-
 	btMultiBodyWorldImporter* worldImporter = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
 
 	btCollisionShape* shape = 0;
@@ -3977,6 +3986,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 	if (clientCmd.m_createUserShapeArgs.m_numUserShapes>1)
 	{
 		compound = worldImporter->createCompoundShape();
+		compound ->setMargin(m_data->m_defaultCollisionMargin);
 	}
 	for (int i = 0; i < clientCmd.m_createUserShapeArgs.m_numUserShapes; i++)
 	{
@@ -4015,6 +4025,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 		{
 			double radius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
 			shape = worldImporter->createSphereShape(radius);
+			shape->setMargin(m_data->m_defaultCollisionMargin);
 			if (compound)
 			{
 				compound->addChildShape(childTransform, shape);
@@ -4031,6 +4042,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 				clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[1],
 				clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[2]);
 			shape = worldImporter->createBoxShape(halfExtents);
+			shape->setMargin(m_data->m_defaultCollisionMargin);
 			if (compound)
 			{
 				compound->addChildShape(childTransform, shape);
@@ -4043,6 +4055,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 		{
 			shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
 				clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
+			shape->setMargin(m_data->m_defaultCollisionMargin);
 			if (compound)
 			{
 				compound->addChildShape(childTransform, shape);
@@ -4057,6 +4070,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 		{
 			shape = worldImporter->createCylinderShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
 				0.5*clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
+			shape->setMargin(m_data->m_defaultCollisionMargin);
 			if (compound)
 			{
 				compound->addChildShape(childTransform, shape);
@@ -4074,6 +4088,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 				clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]);
 
 			shape = worldImporter->createPlaneShape(planeNormal, 0);
+			shape->setMargin(m_data->m_defaultCollisionMargin);
 			if (compound)
 			{
 				compound->addChildShape(childTransform, shape);
@@ -4145,12 +4160,12 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 						{
 							compound = worldImporter->createCompoundShape();
 						}
-						compound->setMargin(defaultCollisionMargin);
+						compound->setMargin(m_data->m_defaultCollisionMargin);
 
 						for (int s = 0; s < (int)shapes.size(); s++)
 						{
 							btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
-							convexHull->setMargin(defaultCollisionMargin);
+							convexHull->setMargin(m_data->m_defaultCollisionMargin);
 							tinyobj::shape_t& shape = shapes[s];
 							int faceCount = shape.mesh.indices.size();
 
@@ -4253,6 +4268,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 						if (compound)
 						{
 							compound->addChildShape(childTransform, shape);
+							shape->setMargin(m_data->m_defaultCollisionMargin);
 						}
 					}
 					delete glmesh;
@@ -4265,11 +4281,11 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 					{
 						compound = worldImporter->createCompoundShape();
 					}
-					compound->setMargin(defaultCollisionMargin);
+					compound->setMargin(m_data->m_defaultCollisionMargin);
 
 					{
 						btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
-						convexHull->setMargin(defaultCollisionMargin);
+						convexHull->setMargin(m_data->m_defaultCollisionMargin);
 
 						for (int v = 0; v < convertedVerts.size(); v++)
 						{
@@ -6235,7 +6251,84 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand
 			btAlignedObjectArray<btCollisionObject*> setB;
 			btAlignedObjectArray<int> setALinkIndex;
 			btAlignedObjectArray<int> setBLinkIndex;
-								
+		
+			btCollisionObject colObA;
+			btCollisionObject colObB;
+			
+			int collisionShapeA = (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_A) ? 
+				clientCmd.m_requestContactPointArguments.m_collisionShapeA : -1;
+			int collisionShapeB = (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_B) ? 
+				clientCmd.m_requestContactPointArguments.m_collisionShapeB : -1;
+
+			if (collisionShapeA>=0)
+			{
+				btVector3 posA(0,0,0);
+				if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_A)
+				{
+					posA.setValue(clientCmd.m_requestContactPointArguments.m_collisionShapePositionA[0],
+						clientCmd.m_requestContactPointArguments.m_collisionShapePositionA[1],
+						clientCmd.m_requestContactPointArguments.m_collisionShapePositionA[2]);
+				}
+				btQuaternion ornA(0,0,0,1);
+				if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_A)
+				{
+					ornA.setValue(clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[0],
+						clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[1],
+						clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[2],
+						clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[3]);
+				}
+				InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeA);
+				
+				if (handle && handle->m_collisionShape)
+				{
+					colObA.setCollisionShape(handle->m_collisionShape);
+					btTransform tr;
+					tr.setIdentity();
+					tr.setOrigin(posA);
+					tr.setRotation(ornA);
+					colObA.setWorldTransform(tr);
+					setA.push_back(&colObA);
+					setALinkIndex.push_back(-2);
+				} else
+				{
+					b3Warning("collisionShapeA provided is not valid.");
+				}
+			}
+			if (collisionShapeB>=0)
+			{
+				btVector3 posB(0,0,0);
+				if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_B)
+				{
+					posB.setValue(clientCmd.m_requestContactPointArguments.m_collisionShapePositionB[0],
+						clientCmd.m_requestContactPointArguments.m_collisionShapePositionB[1],
+						clientCmd.m_requestContactPointArguments.m_collisionShapePositionB[2]);
+				}
+				btQuaternion ornB(0,0,0,1);
+				if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_B)
+				{
+					ornB.setValue(clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationB[0],
+						clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationB[1],
+						clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationB[2],
+						clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationB[3]);
+				}
+
+				InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeB);
+				if (handle && handle->m_collisionShape)
+				{
+					colObB.setCollisionShape(handle->m_collisionShape);
+					btTransform tr;
+					tr.setIdentity();
+					tr.setOrigin(posB);
+					tr.setRotation(ornB);
+					colObB.setWorldTransform(tr);
+					setB.push_back(&colObB);
+					setBLinkIndex.push_back(-2);
+				} else
+				{
+					b3Warning("collisionShapeB provided is not valid.");
+				}
+			}
+
 			if (bodyUniqueIdA >= 0)
 			{
 				InternalBodyData* bodyA = m_data->m_bodyHandles.getHandle(bodyUniqueIdA);
@@ -6358,11 +6451,9 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand
 							m_cachedContactPoints.push_back(pt);
 						}
 						return 1;
-
 					}
 				};
 
-
 				MyContactResultCallback cb(m_data->m_cachedContactPoints);
 
 				cb.m_bodyUniqueIdA = bodyUniqueIdA;
@@ -8585,9 +8676,50 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
 			}
 			m_data->m_bodyHandles.freeHandle(bodyUniqueId);
 		}
-
-		
 	}
+	for (int i=0;i<clientCmd.m_removeObjectArgs.m_numUserCollisionShapes;i++)
+	{
+		int removeCollisionShapeId = clientCmd.m_removeObjectArgs.m_userCollisionShapes[i];
+		InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(removeCollisionShapeId);
+		if (handle && handle->m_collisionShape)
+		{
+			if (handle->m_used)
+			{
+				b3Warning("Don't remove collision shape: it is used.");
+			}
+			else
+			{
+				b3Warning("TODO: dealloc");
+				int foundIndex = -1;
+				
+				for (int i=0;i<m_data->m_worldImporters.size();i++)
+				{
+					btMultiBodyWorldImporter* importer = m_data->m_worldImporters[i];
+					for (int c=0;c<importer->getNumCollisionShapes();c++)
+					{
+						if (importer->getCollisionShapeByIndex(c) == handle->m_collisionShape)
+						{
+							if ( (importer->getNumRigidBodies()==0)&&
+								(importer->getNumConstraints()==0))
+							{
+								foundIndex=i;
+								break;
+							}
+						}
+					}
+				}
+				if (foundIndex>=0)
+				{
+					btMultiBodyWorldImporter* importer = m_data->m_worldImporters[i];
+					m_data->m_worldImporters.removeAtIndex(foundIndex);
+					delete importer;
+					m_data->m_userCollisionShapeHandles.freeHandle(removeCollisionShapeId);
+				}
+			}
+		}
+	}
+	
+	
 	m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,1);
 
 	for (int i=0;i<serverCmd.m_removeObjectArgs.m_numBodies;i++)

examples/SharedMemory/SharedMemoryCommands.h

@@ -278,6 +278,14 @@ enum EnumRequestContactDataUpdateFlags
 	CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD=2,
 	CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER = 4,
 	CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER = 8,
+
+	CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_A = 16,
+	CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_B = 32,
+	CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_A = 64,
+	CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_B = 128,
+	CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_A= 256,
+	CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_B= 512,
+
 };
 
 struct RequestRaycastIntersections
@@ -309,6 +317,14 @@ struct RequestContactDataArgs
 	int m_linkIndexAIndexFilter;
 	int m_linkIndexBIndexFilter;
 	double m_closestDistanceThreshold;
+
+	int m_collisionShapeA;
+	int m_collisionShapeB;
+	double m_collisionShapePositionA[3];
+	double m_collisionShapePositionB[3];
+	double m_collisionShapeOrientationA[4];
+	double m_collisionShapeOrientationB[4];
+	
 	int m_mode;
 };
 
@@ -599,6 +615,8 @@ struct b3ObjectArgs
     int m_bodyUniqueIds[MAX_SDF_BODIES];
 	int m_numUserConstraints;
 	int m_userConstraintUniqueIds[MAX_SDF_BODIES];
+	int m_numUserCollisionShapes;
+	int m_userCollisionShapes[MAX_SDF_BODIES];
 };
 
 struct b3Profile

examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp

@@ -62,6 +62,13 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
 			b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB);
 			b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
 		}
+		if (objectUniqueIdA==objectUniqueIdB)
+		{
+			if (keyValue.m_linkIndexA>keyValue.m_linkIndexB)
+			{
+				b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
+			}
+		}
 
 		m_customCollisionFilters.insert(keyValue,keyValue);
 		
@@ -83,6 +90,13 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
 			b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB);
 			b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
 		}
+		if (objectUniqueIdA==objectUniqueIdB)
+		{
+			if (keyValue.m_linkIndexA>keyValue.m_linkIndexB)
+			{
+				b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
+			}
+		}
 
 		m_customCollisionFilters.remove(keyValue);
 	}
@@ -116,6 +130,13 @@ struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
 			b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB);
 			b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
 		}
+		if (objectUniqueIdA==objectUniqueIdB)
+		{
+			if (keyValue.m_linkIndexA>keyValue.m_linkIndexB)
+			{
+				b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
+			}
+		}
 
 		b3CustomCollisionFilter* filter = m_customCollisionFilters.find(keyValue);
 		if (filter)

examples/SharedMemory/plugins/eglPlugin/eglRendererPlugin.cpp

@@ -62,6 +62,28 @@ B3_SHARED_API struct UrdfRenderingInterface* getRenderInterface_eglRendererPlugi
 
 
 #ifdef EGL_ADD_PYTHON_INIT
+
+static PyMethodDef eglMethods[] = {
+  {NULL, NULL, 0, NULL} /* Sentinel */
+};
+
+#if PY_MAJOR_VERSION >= 3
+static struct PyModuleDef moduledef = {
+        PyModuleDef_HEAD_INIT, "eglRenderer", /* m_name */
+        "eglRenderer for PyBullet "
+        , /* m_doc */
+        -1,            /* m_size */
+        eglMethods,   /* m_methods */
+        NULL,          /* m_reload */
+        NULL,          /* m_traverse */
+        NULL,          /* m_clear */
+        NULL,          /* m_free */
+};
+#endif
+
+
+
+
 PyMODINIT_FUNC
 #if PY_MAJOR_VERSION >= 3
 PyInit_eglRenderer(void)
@@ -69,8 +91,20 @@ PyInit_eglRenderer(void)
 initeglRenderer(void)
 #endif
 {
+
+ PyObject* m;
 #if PY_MAJOR_VERSION >= 3
-	return 0;
+        m = PyModule_Create(&moduledef);
+#else
+        m = Py_InitModule3("eglRenderer", eglMethods, "eglRenderer for PyBullet");
 #endif
+
+#if PY_MAJOR_VERSION >= 3
+        if (m == NULL) return m;
+#else
+        if (m == NULL) return;
+#endif
+
+
 }
 #endif //EGL_ADD_PYTHON_INIT

examples/pybullet/examples/getClosestPoints.py

@@ -0,0 +1,55 @@
+import pybullet as p
+import time
+p.connect(p.GUI)
+useCollisionShapeQuery = True
+p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
+geom = p.createCollisionShape(p.GEOM_SPHERE, radius=0.1)
+geomBox = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2,0.2,0.2])
+baseOrientationB = p.getQuaternionFromEuler([0,0.3,0])#[0,0.5,0.5,0]
+basePositionB = [1.5,0,1]
+obA=-1
+obB=-1
+
+obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom,basePosition=[0.5,0,1])
+obB = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geomBox,basePosition=basePositionB,baseOrientation=baseOrientationB )
+
+
+lineWidth=3
+colorRGB=[1,0,0]
+lineId=p.addUserDebugLine(lineFromXYZ=[0,0,0],lineToXYZ=[0,0,0],lineColorRGB=colorRGB,lineWidth=lineWidth,lifeTime=0)
+pitch=0
+yaw=0
+
+while (p.isConnected()):
+	pitch += 0.01
+	if (pitch>=3.1415*2.):
+		pitch=0
+	yaw+= 0.01
+	if (yaw>=3.1415*2.):
+		yaw=0
+		
+	baseOrientationB = p.getQuaternionFromEuler([yaw,pitch,0])
+	if (obB>=0):
+		p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB)
+
+	if (useCollisionShapeQuery):
+		pts = p.getClosestPoints(bodyA=-1, bodyB=-1, distance=100, collisionShapeA=geom,collisionShapeB=geomBox, collisionShapePositionA=[0.5,0,1],collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB)
+		#pts = p.getClosestPoints(bodyA=obA, bodyB=-1, distance=100, collisionShapeB=geomBox, collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB)
+	else:
+		pts = p.getClosestPoints(bodyA=obA, bodyB=obB, distance=100)
+
+	if len(pts)>0:
+		#print(pts)	
+		distance = pts[0][8]
+		#print("distance=",distance)
+		ptA = pts[0][5]
+		ptB = pts[0][6]
+		p.addUserDebugLine(lineFromXYZ=ptA,lineToXYZ=ptB,lineColorRGB=colorRGB,lineWidth=lineWidth,lifeTime=0,replaceItemUniqueId=lineId); 
+	#time.sleep(1./240.)
+	
+
+#removeCollisionShape is optional:
+#only use removeCollisionShape if the collision shape is not used to create a body
+#and if you want to keep on creating new collision shapes for different queries (not recommended)
+p.removeCollisionShape(geom)
+p.removeCollisionShape(geomBox)

examples/pybullet/pybullet.c

@@ -3099,6 +3099,40 @@ static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args, PyObje
 	}
 }
 
+static PyObject* pybullet_removeCollisionShape(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	{
+		int collisionShapeId= -1;
+		b3PhysicsClientHandle sm = 0;
+
+		int physicsClientId = 0;
+		static char* kwlist[] = {"collisionShapeId", "physicsClientId", NULL};
+		if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &collisionShapeId, &physicsClientId))
+		{
+			return NULL;
+		}
+		sm = getPhysicsClient(physicsClientId);
+		if (sm == 0)
+		{
+			PyErr_SetString(SpamError, "Not connected to physics server.");
+			return NULL;
+		}
+		if (collisionShapeId>=0)
+		{
+			b3SharedMemoryStatusHandle statusHandle;
+			int statusType;
+			if (b3CanSubmitCommand(sm))
+			{
+				statusHandle = b3SubmitClientCommandAndWaitStatus( sm, b3InitRemoveCollisionShapeCommand(sm,collisionShapeId));
+				statusType = b3GetStatusType(statusHandle);
+			}
+		}
+	}
+
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
 static PyObject* pybullet_removeBody(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	{
@@ -6002,6 +6036,17 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
 	int bodyUniqueIdB = -1;
 	int linkIndexA = -2;
 	int linkIndexB = -2;
+	int collisionShapeA = -1;
+	int collisionShapeB = -1;
+
+	PyObject* collisionShapePositionAObj = 0;
+	PyObject* collisionShapeOrientationAObj = 0;
+	double collisionShapePositionA[3]={0,0,0};
+	double collisionShapeOrientationA[4]={0,0,0,1};
+	PyObject* collisionShapePositionBObj = 0;
+	PyObject* collisionShapeOrientationBObj = 0;
+	double collisionShapePositionB[3]={0,0,0};
+	double collisionShapeOrientationB[4]={0,0,0,1};
 
 	double distanceThreshold = 0.f;
 
@@ -6011,10 +6056,14 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
 	int statusType;
 	int physicsClientId = 0;
 	b3PhysicsClientHandle sm = 0;
-	static char* kwlist[] = {"bodyA", "bodyB", "distance", "linkIndexA", "linkIndexB", "physicsClientId", NULL};
+	static char* kwlist[] = {"bodyA", "bodyB", "distance", "linkIndexA", "linkIndexB", "collisionShapeA","collisionShapeB", "collisionShapePositionA","collisionShapePositionB","collisionShapeOrientationA","collisionShapeOrientationB","physicsClientId", NULL};
 
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|iii", kwlist,
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|iiiiOOOOi", kwlist,
-									 &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB, &physicsClientId))
+									 &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB, 
+									 &collisionShapeA, &collisionShapeB,
+									 &collisionShapePositionAObj,&collisionShapePositionBObj,
+									 &collisionShapeOrientationA,&collisionShapeOrientationBObj,
+									 &physicsClientId))
 		return NULL;
 
 	sm = getPhysicsClient(physicsClientId);
@@ -6025,8 +6074,14 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
 	}
 
 	commandHandle = b3InitClosestDistanceQuery(sm);
-	b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA);
+	if (bodyUniqueIdA>=0)
-	b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB);
+	{
+		b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA);
+	}
+	if (bodyUniqueIdB>=0)
+	{
+		b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB);
+	}
 	b3SetClosestDistanceThreshold(commandHandle, distanceThreshold);
 	if (linkIndexA >= -1)
 	{
@@ -6036,6 +6091,34 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
 	{
 		b3SetClosestDistanceFilterLinkB(commandHandle, linkIndexB);
 	}
+	if (collisionShapeA>=0)
+	{
+		b3SetClosestDistanceFilterCollisionShapeA(commandHandle, collisionShapeA);
+	}
+	if (collisionShapeB>=0)
+	{
+		b3SetClosestDistanceFilterCollisionShapeB(commandHandle, collisionShapeB);
+	}
+	if (collisionShapePositionAObj)
+	{
+		pybullet_internalSetVectord(collisionShapePositionAObj,collisionShapePositionA);
+		b3SetClosestDistanceFilterCollisionShapePositionA(commandHandle, collisionShapePositionA);
+	}
+	if (collisionShapePositionBObj)
+	{
+		pybullet_internalSetVectord(collisionShapePositionBObj,collisionShapePositionB);
+		b3SetClosestDistanceFilterCollisionShapePositionB(commandHandle, collisionShapePositionB);
+	}
+	if (collisionShapeOrientationAObj)
+	{
+		pybullet_internalSetVector4d(collisionShapeOrientationAObj,collisionShapeOrientationA);
+		b3SetClosestDistanceFilterCollisionShapeOrientationA(commandHandle, collisionShapeOrientationA);
+	}
+	if (collisionShapeOrientationBObj)
+	{
+		pybullet_internalSetVector4d(collisionShapeOrientationBObj,collisionShapeOrientationB);
+		b3SetClosestDistanceFilterCollisionShapeOrientationB(commandHandle, collisionShapeOrientationB);
+	}
 
 	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
 	statusType = b3GetStatusType(statusHandle);
@@ -9244,6 +9327,9 @@ static PyMethodDef SpamMethods[] = {
 	{ "createCollisionShapeArray", (PyCFunction)pybullet_createCollisionShapeArray, METH_VARARGS | METH_KEYWORDS,
 	"Create collision shapes. Returns a non-negative (int) unique id, if successfull, negative otherwise." },
 
+	{"removeCollisionShape", (PyCFunction)pybullet_removeCollisionShape, METH_VARARGS | METH_KEYWORDS,
+	 "Remove a collision shape. Only useful when the collision shape is not used in a body (to perform a getClosestPoint query)."},
+	 
 	{"createVisualShape", (PyCFunction)pybullet_createVisualShape, METH_VARARGS | METH_KEYWORDS,
 	 "Create a visual shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."},
 

setup.py

@@ -42,7 +42,7 @@ CXX_FLAGS += '-DBT_USE_DOUBLE_PRECISION '
 CXX_FLAGS += '-DBT_ENABLE_ENET '
 CXX_FLAGS += '-DBT_ENABLE_CLSOCKET '
 CXX_FLAGS += '-DB3_DUMP_PYTHON_VERSION '
-
+CXX_FLAGS += '-DEGL_ADD_PYTHON_INIT '
 EGL_CXX_FLAGS = ''
 
 
@@ -535,12 +535,26 @@ print("packages")
 print(find_packages('examples/pybullet/gym'))
 print("-----")
 
-eglRender = Extension("eglRenderer",
+extensions = []
-        sources =  egl_renderer_sources,
+
+pybullet_ext = Extension("pybullet",
+        sources =  sources,
         libraries = libraries,
-        extra_compile_args=(CXX_FLAGS+EGL_CXX_FLAGS ).split(),
+        extra_compile_args=CXX_FLAGS.split(),
-        include_dirs = include_dirs + ["src","examples", "examples/ThirdPartyLibs","examples/ThirdPartyLibs/glad", "examples/ThirdPartyLibs/enet/include","examples/ThirdPartyLibs/clsocket/src"]
+        include_dirs = include_dirs + ["src","examples/ThirdPartyLibs","examples/ThirdPartyLibs/glad", "examples/ThirdPartyLibs/enet/include","examples/ThirdPartyLibs/clsocket/src"]
      )
+extensions.append(pybullet_ext)
+
+
+if 'BT_USE_EGL' in EGL_CXX_FLAGS:
+
+	eglRender = Extension("eglRenderer",
+        	sources =  egl_renderer_sources,
+        	libraries = libraries,
+        	extra_compile_args=(CXX_FLAGS+EGL_CXX_FLAGS ).split(),
+        	include_dirs = include_dirs + ["src","examples", "examples/ThirdPartyLibs","examples/ThirdPartyLibs/glad", "examples/ThirdPartyLibs/enet/include","examples/ThirdPartyLibs/clsocket/src"])
+
+	extensions.append(eglRender)
 
 
 setup(
@@ -554,13 +568,8 @@ setup(
 	license='zlib',
 	platforms='any',
 	keywords=['game development', 'virtual reality', 'physics simulation', 'robotics', 'collision detection', 'opengl'],
-	ext_modules = [eglRender, Extension("pybullet",
+	ext_modules = extensions,
-	sources =  sources,
+	classifiers=['Development Status :: 5 - Production/Stable',
-	libraries = libraries,
-	extra_compile_args=CXX_FLAGS.split(),
-	include_dirs = include_dirs + ["src","examples/ThirdPartyLibs","examples/ThirdPartyLibs/glad", "examples/ThirdPartyLibs/enet/include","examples/ThirdPartyLibs/clsocket/src"]
-     ) ],
-     classifiers=['Development Status :: 5 - Production/Stable',
                    'License :: OSI Approved :: zlib/libpng License',
                    'Operating System :: Microsoft :: Windows',
                    'Operating System :: POSIX :: Linux',