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/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."},