add getClosestPoints.py example.

allow to perform a getClosestPoints query with a collisionShape and world transform (position, orientation) that isn't part of the world.
(use createCollisionShape to create it)
add optional removeCollisionShape, for collision shapes only used in a query (and not used to create a body)

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;

examples/SharedMemory/PhysicsClientC_API.h

@@ -496,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;
 	}
 };
 
@@ -2383,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;
@@ -6270,6 +6278,7 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand
 						clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[3]);
 				}
 				InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeA);
+				
 				if (handle && handle->m_collisionShape)
 				{
 					colObA.setCollisionShape(handle->m_collisionShape);
@@ -6280,6 +6289,9 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand
 					colObA.setWorldTransform(tr);
 					setA.push_back(&colObA);
 					setALinkIndex.push_back(-2);
+				} else
+				{
+					b3Warning("collisionShapeA provided is not valid.");
 				}
 			}
 			if (collisionShapeB>=0)
@@ -6311,6 +6323,9 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand
 					colObB.setWorldTransform(tr);
 					setB.push_back(&colObB);
 					setBLinkIndex.push_back(-2);
+				} else
+				{
+					b3Warning("collisionShapeB provided is not valid.");
 				}
 			}
 
@@ -6436,11 +6451,9 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand
 							m_cachedContactPoints.push_back(pt);
 						}
 						return 1;
-
 					}
 				};
 
-
 				MyContactResultCallback cb(m_data->m_cachedContactPoints);
 
 				cb.m_bodyUniqueIdA = bodyUniqueIdA;
@@ -8663,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

@@ -615,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/pybullet/examples/getClosestPoints.py

@@ -1,29 +1,40 @@
 import pybullet as p
 import time
 p.connect(p.GUI)
-useCollisionShapeQuery = False
-
+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])
-obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom,basePosition=[0.5,0,1])
 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([0,pitch,0])#[0,0.5,0.5,0]
-	p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB)
+	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)
 
@@ -34,5 +45,11 @@ while (p.isConnected()):
 		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.)
-	
+	#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)
 {
 	{
@@ -9293,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."},