regular OR wireframe rendering, not both

add option to perform filtering of 'getClosestPoints' using linkA/linkB.
don't use 'realtimesimulation' as default
add/remove debug items within same thread
pybullet, report contact points and normal as [x,y,z] triplet/vector, not 3 scalars
separate 'getClosestPointsAlgorithm': box-box doesn't report closest points with positive distance, and the query shouldn't impact regular 'closesst points'

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -1167,7 +1167,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
 				}
                 BT_PROFILE("Render Scene");
                 sCurrentDemo->renderScene();
-            }
+            } else
             {
 				B3_PROFILE("physicsDebugDraw");
 				glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -1504,6 +1504,8 @@ b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClient
 	command->m_requestContactPointArguments.m_startingContactPointIndex = 0;
 	command->m_requestContactPointArguments.m_objectAIndexFilter = -1;
 	command->m_requestContactPointArguments.m_objectBIndexFilter = -1;
+	command->m_requestContactPointArguments.m_linkIndexAIndexFilter = -2;
+	command->m_requestContactPointArguments.m_linkIndexBIndexFilter = -2;
     command->m_updateFlags = 0;
     return (b3SharedMemoryCommandHandle) command;
 }
@@ -1516,6 +1518,37 @@ void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int body
 	command->m_requestContactPointArguments.m_objectAIndexFilter = bodyUniqueIdA;
 }
 
+void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA)
+{
+	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_LINK_INDEX_A_FILTER;
+	command->m_requestContactPointArguments.m_linkIndexAIndexFilter= linkIndexA;
+}
+
+void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB)
+{
+	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_LINK_INDEX_B_FILTER;
+	command->m_requestContactPointArguments.m_linkIndexBIndexFilter = linkIndexB;
+}
+
+void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA)
+{
+	b3SetContactFilterLinkA(commandHandle, linkIndexA);
+}
+
+void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB)
+{
+	b3SetContactFilterLinkB(commandHandle, linkIndexB);
+}
+
+
+
+
 void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

examples/SharedMemory/PhysicsClientC_API.h

@@ -123,12 +123,16 @@ void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle comm
 b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient);
 void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
 void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
+void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
+void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
 void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
 
 ///compute the closest points between two bodies
 b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient);
 void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
+void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
 void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
+void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
 void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance);
 void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -542,7 +542,7 @@ struct PhysicsServerCommandProcessorInternalData
 		m_kukaGripperMultiBody(0),
 		m_kukaGripperRevolute1(0),
 		m_kukaGripperRevolute2(0),
-		m_allowRealTimeSimulation(true),
+		m_allowRealTimeSimulation(false),
 		m_huskyId(-1),
 		m_KukaId(-1),
 		m_sphereId(-1),
@@ -2769,6 +2769,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								int bodyUniqueIdA = clientCmd.m_requestContactPointArguments.m_objectAIndexFilter;
 								int bodyUniqueIdB = clientCmd.m_requestContactPointArguments.m_objectBIndexFilter;
 
+								bool hasLinkIndexAFilter = (0!=(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER));
+								bool hasLinkIndexBFilter = (0!=(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER));
+
+								int linkIndexA  = clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter;
+								int linkIndexB = clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter;
+
 								btAlignedObjectArray<btCollisionObject*> setA;
 								btAlignedObjectArray<btCollisionObject*> setB;
 								btAlignedObjectArray<int> setALinkIndex;
@@ -2783,15 +2789,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 										{
 											if (bodyA->m_multiBody->getBaseCollider())
 											{
-												setA.push_back(bodyA->m_multiBody->getBaseCollider());
+												if (!hasLinkIndexAFilter || (linkIndexA == -1))
-												setALinkIndex.push_back(-1);
+												{
+													setA.push_back(bodyA->m_multiBody->getBaseCollider());
+													setALinkIndex.push_back(-1);
+												}
 											}
 											for (int i = 0; i < bodyA->m_multiBody->getNumLinks(); i++)
 											{
 												if (bodyA->m_multiBody->getLink(i).m_collider)
 												{
-													setA.push_back(bodyA->m_multiBody->getLink(i).m_collider);
+													if (!hasLinkIndexAFilter || (linkIndexA == i))
-													setALinkIndex.push_back(i);
+													{
+														setA.push_back(bodyA->m_multiBody->getLink(i).m_collider);
+														setALinkIndex.push_back(i);
+													}
 												}
 											}
 										}
@@ -2811,15 +2823,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 										{
 											if (bodyB->m_multiBody->getBaseCollider())
 											{
-												setB.push_back(bodyB->m_multiBody->getBaseCollider());
+												if (!hasLinkIndexBFilter || (linkIndexB == -1))
-												setBLinkIndex.push_back(-1);
+												{
+													setB.push_back(bodyB->m_multiBody->getBaseCollider());
+													setBLinkIndex.push_back(-1);
+												}
 											}
 											for (int i = 0; i < bodyB->m_multiBody->getNumLinks(); i++)
 											{
 												if (bodyB->m_multiBody->getLink(i).m_collider)
 												{
-													setB.push_back(bodyB->m_multiBody->getLink(i).m_collider);
+													if (!hasLinkIndexBFilter || (linkIndexB ==i))
-													setBLinkIndex.push_back(i);
+													{
+														setB.push_back(bodyB->m_multiBody->getLink(i).m_collider);
+														setBLinkIndex.push_back(i);
+													}
 												}
 											}
 										}

examples/SharedMemory/PhysicsServerExample.cpp

@@ -126,10 +126,6 @@ enum MultiThreadedGUIHelperCommunicationEnums
 	eGUIHelperRemoveAllGraphicsInstances,
 	eGUIHelperCopyCameraImageData,
 	eGUIHelperAutogenerateGraphicsObjects,
-	eGUIUserDebugAddText,
-	eGUIUserDebugAddLine,
-	eGUIUserDebugRemoveItem,
-	eGUIUserDebugRemoveAllItems,
 };
 
 #include <stdio.h>
@@ -701,43 +697,36 @@ public:
 		m_tmpText.m_textColorRGB[2] = textColorRGB[2];
 
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIUserDebugAddText);
+		m_userDebugText.push_back(m_tmpText);
 		m_cs->unlock();
-		while (m_cs->getSharedParam(1) != eGUIHelperIdle)
-		{
-			b3Clock::usleep(150);
-		}
 
 		return m_userDebugText[m_userDebugText.size()-1].m_itemUniqueId;
 	}
 
 	btAlignedObjectArray<UserDebugDrawLine> m_userDebugLines;
-	UserDebugDrawLine m_tmpLine;
 
 	virtual int		addUserDebugLine(const double	debugLineFromXYZ[3], const double	debugLineToXYZ[3], const double	debugLineColorRGB[3], double lineWidth, double lifeTime )
 	{
-		m_tmpLine.m_lifeTime = lifeTime;
+		UserDebugDrawLine tmpLine;
-		m_tmpLine.m_lineWidth = lineWidth;
-		m_tmpLine.m_itemUniqueId = m_uidGenerator++;
-		m_tmpLine.m_debugLineFromXYZ[0] = debugLineFromXYZ[0];
-		m_tmpLine.m_debugLineFromXYZ[1] = debugLineFromXYZ[1];
-		m_tmpLine.m_debugLineFromXYZ[2] = debugLineFromXYZ[2];
 
-		m_tmpLine.m_debugLineToXYZ[0] = debugLineToXYZ[0];
+		tmpLine.m_lifeTime = lifeTime;
-		m_tmpLine.m_debugLineToXYZ[1] = debugLineToXYZ[1];
+		tmpLine.m_lineWidth = lineWidth;
-		m_tmpLine.m_debugLineToXYZ[2] = debugLineToXYZ[2];
+		tmpLine.m_itemUniqueId = m_uidGenerator++;
+		tmpLine.m_debugLineFromXYZ[0] = debugLineFromXYZ[0];
+		tmpLine.m_debugLineFromXYZ[1] = debugLineFromXYZ[1];
+		tmpLine.m_debugLineFromXYZ[2] = debugLineFromXYZ[2];
+
+		tmpLine.m_debugLineToXYZ[0] = debugLineToXYZ[0];
+		tmpLine.m_debugLineToXYZ[1] = debugLineToXYZ[1];
+		tmpLine.m_debugLineToXYZ[2] = debugLineToXYZ[2];
 		
-		m_tmpLine.m_debugLineColorRGB[0] = debugLineColorRGB[0];
+		tmpLine.m_debugLineColorRGB[0] = debugLineColorRGB[0];
-		m_tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1];
+		tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1];
-		m_tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2];
+		tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2];
 		
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIUserDebugAddLine);
+		m_userDebugLines.push_back(tmpLine);
 		m_cs->unlock();
-		while (m_cs->getSharedParam(1) != eGUIHelperIdle)
-		{
-			b3Clock::usleep(150);
-		}
 		return m_userDebugLines[m_userDebugLines.size()-1].m_itemUniqueId;
 	}
 
@@ -747,23 +736,38 @@ public:
 	{
 		m_removeDebugItemUid = debugItemUniqueId;
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIUserDebugRemoveItem);
+
-		m_cs->unlock();
+		for (int i = 0; i<m_userDebugLines.size(); i++)
-		while (m_cs->getSharedParam(1) != eGUIHelperIdle)
 		{
-			b3Clock::usleep(150);
+			if (m_userDebugLines[i].m_itemUniqueId == m_removeDebugItemUid)
+			{
+				m_userDebugLines.swap(i, m_userDebugLines.size() - 1);
+				m_userDebugLines.pop_back();
+				break;
+			}
 		}
 
+
+		for (int i = 0; i<m_userDebugText.size(); i++)
+		{
+			if (m_userDebugText[i].m_itemUniqueId == m_removeDebugItemUid)
+			{
+				m_userDebugText.swap(i, m_userDebugText.size() - 1);
+				m_userDebugText.pop_back();
+				break;
+			}
+		}
+
+		m_cs->unlock();
+
 	}	
 	virtual void	removeAllUserDebugItems( )
 	{
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIUserDebugRemoveAllItems);
+		m_userDebugLines.clear();
+		m_userDebugText.clear();
+		m_uidGenerator = 0;
 		m_cs->unlock();
-		while (m_cs->getSharedParam(1) != eGUIHelperIdle)
-		{
-			b3Clock::usleep(150);
-		}
 	
 	}
 
@@ -825,6 +829,7 @@ public:
     virtual bool wantsTermination();
     virtual bool isConnected();
 	virtual void	renderScene();
+	void drawUserDebugLines();
 	virtual void    exitPhysics();
 
 	virtual void	physicsDebugDraw(int debugFlags);
@@ -1247,60 +1252,6 @@ void	PhysicsServerExample::stepSimulation(float deltaTime)
 		break;
 	}
 
-	case eGUIUserDebugAddText:
-	{
-		m_multiThreadedHelper->m_userDebugText.push_back(m_multiThreadedHelper->m_tmpText);
-		m_multiThreadedHelper->getCriticalSection()->lock();
-		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
-		m_multiThreadedHelper->getCriticalSection()->unlock();
-		break;
-	}
-	case eGUIUserDebugAddLine:
-	{
-		m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine);
-		m_multiThreadedHelper->getCriticalSection()->lock();
-		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
-		m_multiThreadedHelper->getCriticalSection()->unlock();
-			break;
-	}
-	case eGUIUserDebugRemoveItem:
-	{
-		for (int i=0;i<m_multiThreadedHelper->m_userDebugLines.size();i++)
-		{
-			if (m_multiThreadedHelper->m_userDebugLines[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid)
-			{
-				m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1);
-				m_multiThreadedHelper->m_userDebugLines.pop_back();
-				break;
-			}
-		}
-
-
-		for (int i=0;i<m_multiThreadedHelper->m_userDebugText.size();i++)
-		{
-			if (m_multiThreadedHelper->m_userDebugText[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid)
-			{
-				m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1);
-				m_multiThreadedHelper->m_userDebugText.pop_back();
-				break;
-			}
-		}
-
-		m_multiThreadedHelper->getCriticalSection()->lock();
-		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
-		m_multiThreadedHelper->getCriticalSection()->unlock();
-			break;
-	}
-	case eGUIUserDebugRemoveAllItems:
-	{
-		m_multiThreadedHelper->m_userDebugLines.clear();
-		m_multiThreadedHelper->m_userDebugText.clear();
-		m_multiThreadedHelper->m_uidGenerator = 0;
-		m_multiThreadedHelper->getCriticalSection()->lock();
-		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
-		m_multiThreadedHelper->getCriticalSection()->unlock();
-			break;
-	}
 	case eGUIHelperIdle:
 		{
 			break;
@@ -1353,7 +1304,53 @@ extern int gHuskyId;
 extern btTransform huskyTr;
 
 
+void PhysicsServerExample::drawUserDebugLines()
+{
+	static char line0[1024];
+	static char line1[1024];
 
+	//draw all user-debug-lines
+
+	//add array of lines
+
+	//draw all user- 'text3d' messages
+	if (m_multiThreadedHelper)
+	{
+		m_args[0].m_cs->lock();
+		for (int i = 0; i<m_multiThreadedHelper->m_userDebugLines.size(); i++)
+		{
+			btVector3 from;
+			from.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[0],
+				m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[1],
+				m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[2]);
+			btVector3 toX;
+			toX.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[0],
+				m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[1],
+				m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[2]);
+
+			btVector3 color;
+			color.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0],
+				m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1],
+				m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[2]);
+
+
+			m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth);
+		}
+
+		for (int i = 0; i<m_multiThreadedHelper->m_userDebugText.size(); i++)
+		{
+			m_guiHelper->getAppInterface()->drawText3D(m_multiThreadedHelper->m_userDebugText[i].m_text,
+				m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[0],
+				m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[1],
+				m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[2],
+				m_multiThreadedHelper->m_userDebugText[i].textSize);
+
+		}
+		m_args[0].m_cs->unlock();
+
+	}
+
+}
 
 void PhysicsServerExample::renderScene()
 {
@@ -1369,48 +1366,8 @@ void PhysicsServerExample::renderScene()
 		
 
 	B3_PROFILE("PhysicsServerExample::RenderScene");
-	static char line0[1024];
-		static char line1[1024];
-
-	//draw all user-debug-lines
-
-	//add array of lines
-
-	//draw all user- 'text3d' messages
-		if (m_multiThreadedHelper)
-		{
-			
-			for (int i=0;i<m_multiThreadedHelper->m_userDebugLines.size();i++)
-			{
-				btVector3 from;
-				from.setValue(	m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[0],
-								m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[1],
-								m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[2]);
-				btVector3 toX;
-				toX.setValue(	m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[0],
-								m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[1],
-								m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[2]);
-				
-				btVector3 color;
-				color.setValue(	m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0],
-								m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1],
-								m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[2]);
-
-				
-				m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth);
-			}
-			
-			for (int i=0;i<m_multiThreadedHelper->m_userDebugText.size();i++)
-			{
-				m_guiHelper->getAppInterface()->drawText3D(m_multiThreadedHelper->m_userDebugText[i].m_text,
-					m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[0],
-					m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[1],
-					m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[2],
-					m_multiThreadedHelper->m_userDebugText[i].textSize);
-				
-			}
-		}
 
+	drawUserDebugLines();
 
 	if (gEnableRealTimeSimVR)
 	{
@@ -1424,6 +1381,7 @@ void PhysicsServerExample::renderScene()
 		static int count = 0;
 		count++;
 
+#if 0
 		if (0 == (count & 1))
 		{
 			btScalar curTime = m_clock.getTimeSeconds();
@@ -1444,6 +1402,7 @@ void PhysicsServerExample::renderScene()
 				worseFps = 1000000;
 			}
 		}
+#endif
 
 #ifdef BT_ENABLE_VR
 		if ((gInternalSimFlags&2 ) && m_tinyVrGui==0)
@@ -1544,6 +1503,8 @@ void PhysicsServerExample::renderScene()
 
 void    PhysicsServerExample::physicsDebugDraw(int debugDrawFlags)
 {
+	drawUserDebugLines();
+
 	///debug rendering
 	m_physicsServer.physicsDebugDraw(debugDrawFlags);
 

examples/SharedMemory/SharedMemoryCommands.h

@@ -154,6 +154,8 @@ enum EnumRequestContactDataUpdateFlags
 {
 	CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE=1,
 	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,
 };
 
 struct RequestContactDataArgs
@@ -161,6 +163,8 @@ struct RequestContactDataArgs
     int m_startingContactPointIndex;
     int m_objectAIndexFilter;
 	int m_objectBIndexFilter;
+	int m_linkIndexAIndexFilter;
+	int m_linkIndexBIndexFilter;
 	double m_closestDistanceThreshold;
 	int m_mode;
 };

examples/pybullet/pybullet.c

@@ -1501,7 +1501,7 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj
   res = pybullet_internalSetVectord(textPositionObj,posXYZ);
   if (!res)
   {
-	  PyErr_SetString(SpamError, "Error converting lineFrom[3]");
+	  PyErr_SetString(SpamError, "Error converting textPositionObj[3]");
 	  return NULL;
   }
 
@@ -1510,7 +1510,7 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj
 	  res = pybullet_internalSetVectord(textColorRGBObj,colorRGB);
 	  if (!res)
 	  {
-		  PyErr_SetString(SpamError, "Error converting lineTo[3]");
+		  PyErr_SetString(SpamError, "Error converting textColorRGBObj[3]");
 		  return NULL;
 	  }
   }
@@ -1848,23 +1848,22 @@ static PyObject* MyConvertContactPoint( struct b3ContactInformation* contactPoin
      2     int m_bodyUniqueIdB;
      3     int m_linkIndexA;
      4     int m_linkIndexB;
-     5-6-7     double m_positionOnAInWS[3];//contact point location on object A,
+     5		double m_positionOnAInWS[3];//contact point location on object A,
      in world space coordinates
-     8-9-10     double m_positionOnBInWS[3];//contact point location on object
+     6		double m_positionOnBInWS[3];//contact point location on object
      A, in world space coordinates
-     11-12-13     double m_contactNormalOnBInWS[3];//the separating contact
+     7		double m_contactNormalOnBInWS[3];//the separating contact
      normal, pointing from object B towards object A
-     14     double m_contactDistance;//negative number is penetration, positive
+     8		double m_contactDistance;//negative number is penetration, positive
      is distance.
-
+     9		double m_normalForce;
-     15    double m_normalForce;
      */
 
 	int i;
 
 	PyObject* pyResultList = PyTuple_New(contactPointPtr->m_numContactPoints);
     for (i = 0; i < contactPointPtr->m_numContactPoints; i++) {
-      PyObject* contactObList = PyTuple_New(16);  // see above 16 fields
+      PyObject* contactObList = PyTuple_New(10);  // see above 10 fields
       PyObject* item;
       item =
           PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_contactFlags);
@@ -1881,42 +1880,61 @@ static PyObject* MyConvertContactPoint( struct b3ContactInformation* contactPoin
       item =
           PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexB);
       PyTuple_SetItem(contactObList, 4, item);
-      item = PyFloat_FromDouble(
-          contactPointPtr->m_contactPointData[i].m_positionOnAInWS[0]);
-      PyTuple_SetItem(contactObList, 5, item);
-      item = PyFloat_FromDouble(
-          contactPointPtr->m_contactPointData[i].m_positionOnAInWS[1]);
-      PyTuple_SetItem(contactObList, 6, item);
-      item = PyFloat_FromDouble(
-          contactPointPtr->m_contactPointData[i].m_positionOnAInWS[2]);
-      PyTuple_SetItem(contactObList, 7, item);
 
-      item = PyFloat_FromDouble(
+	  {
-          contactPointPtr->m_contactPointData[i].m_positionOnBInWS[0]);
+		  PyObject* posAObj = PyTuple_New(3);
-      PyTuple_SetItem(contactObList, 8, item);
-      item = PyFloat_FromDouble(
-          contactPointPtr->m_contactPointData[i].m_positionOnBInWS[1]);
-      PyTuple_SetItem(contactObList, 9, item);
-      item = PyFloat_FromDouble(
-          contactPointPtr->m_contactPointData[i].m_positionOnBInWS[2]);
-      PyTuple_SetItem(contactObList, 10, item);
 
-      item = PyFloat_FromDouble(
+		  item = PyFloat_FromDouble(
-          contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[0]);
+			  contactPointPtr->m_contactPointData[i].m_positionOnAInWS[0]);
-      PyTuple_SetItem(contactObList, 11, item);
+		  PyTuple_SetItem(posAObj, 0, item);
-      item = PyFloat_FromDouble(
+		  item = PyFloat_FromDouble(
-          contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[1]);
+			  contactPointPtr->m_contactPointData[i].m_positionOnAInWS[1]);
-      PyTuple_SetItem(contactObList, 12, item);
+		  PyTuple_SetItem(posAObj, 1, item);
-      item = PyFloat_FromDouble(
+		  item = PyFloat_FromDouble(
-          contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[2]);
+			  contactPointPtr->m_contactPointData[i].m_positionOnAInWS[2]);
-      PyTuple_SetItem(contactObList, 13, item);
+		  PyTuple_SetItem(posAObj, 2, item);
+
+		  PyTuple_SetItem(contactObList, 5, posAObj);
+	  }
+
+	  {
+		  PyObject* posBObj = PyTuple_New(3);
+
+
+		  item = PyFloat_FromDouble(
+			  contactPointPtr->m_contactPointData[i].m_positionOnBInWS[0]);
+		  PyTuple_SetItem(posBObj, 0, item);
+		  item = PyFloat_FromDouble(
+			  contactPointPtr->m_contactPointData[i].m_positionOnBInWS[1]);
+		  PyTuple_SetItem(posBObj, 1, item);
+		  item = PyFloat_FromDouble(
+			  contactPointPtr->m_contactPointData[i].m_positionOnBInWS[2]);
+		  PyTuple_SetItem(posBObj, 2, item);
+
+		  PyTuple_SetItem(contactObList, 6, posBObj);
+
+	  }
+
+	  {
+		  PyObject* normalOnB = PyTuple_New(3);
+		  item = PyFloat_FromDouble(
+			  contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[0]);
+		  PyTuple_SetItem(normalOnB, 0, item);
+		  item = PyFloat_FromDouble(
+			  contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[1]);
+		  PyTuple_SetItem(normalOnB, 1, item);
+		  item = PyFloat_FromDouble(
+			  contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[2]);
+		  PyTuple_SetItem(normalOnB, 2, item);
+		  PyTuple_SetItem(contactObList, 7, normalOnB);
+	  }
 
       item = PyFloat_FromDouble(
           contactPointPtr->m_contactPointData[i].m_contactDistance);
-      PyTuple_SetItem(contactObList, 14, item);
+      PyTuple_SetItem(contactObList, 8, item);
       item = PyFloat_FromDouble(
           contactPointPtr->m_contactPointData[i].m_normalForce);
-      PyTuple_SetItem(contactObList, 15, item);
+      PyTuple_SetItem(contactObList, 9, item);
 
       PyTuple_SetItem(pyResultList, i, contactObList);
     }
@@ -1982,6 +2000,9 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
   int size = PySequence_Size(args);
   int bodyUniqueIdA = -1;
   int bodyUniqueIdB = -1;
+  int linkIndexA = -2;
+  int linkIndexB = -2;
+
   double distanceThreshold = 0.f;
 
   b3SharedMemoryCommandHandle commandHandle;
@@ -1991,15 +2012,15 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
   PyObject* pyResultList = 0;
 
 
-  static char *kwlist[] = { "bodyA", "bodyB", "distance", NULL };
+  static char *kwlist[] = { "bodyA", "bodyB", "distance", "linkIndexA","linkIndexB",NULL };
 
   if (0 == sm) {
 	  PyErr_SetString(SpamError, "Not connected to physics server.");
 	  return NULL;
   }
 
-  if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid", kwlist,
+  if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|ii", kwlist,
-	  &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold))
+	  &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB))
 	  return NULL;
 
 
@@ -2007,7 +2028,14 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
   b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA);
   b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB);
   b3SetClosestDistanceThreshold(commandHandle, distanceThreshold);
-  
+  if (linkIndexA >= -1)
+  {
+	  b3SetClosestDistanceFilterLinkA(commandHandle, linkIndexA);
+  }
+  if (linkIndexB >= -1)
+  {
+	  b3SetClosestDistanceFilterLinkB(commandHandle, linkIndexB);
+  }
 
   statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
   statusType = b3GetStatusType(statusHandle);

src/BulletCollision/BroadphaseCollision/btDispatcher.h

@@ -64,6 +64,12 @@ struct btDispatcherInfo
 	btScalar	m_convexConservativeDistanceThreshold;
 };
 
+enum ebtDispatcherQueryType
+{
+	BT_CONTACT_POINT_ALGORITHMS = 1,
+	BT_CLOSEST_POINT_ALGORITHMS = 2
+};
+
 ///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs.
 ///For example for pairwise collision detection, calculating contact points stored in btPersistentManifold or user callbacks (game logic).
 class btDispatcher
@@ -73,7 +79,7 @@ class btDispatcher
 public:
 	virtual ~btDispatcher() ;
 
-	virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold=0) = 0;
+	virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType) = 0;
 
 	virtual btPersistentManifold*	getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1)=0;
 

src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h

@@ -40,6 +40,9 @@ public:
 
 	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0;
 
+	virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) = 0;
+
+
 };
 
 #endif //BT_COLLISION_CONFIGURATION

src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -50,8 +50,10 @@ m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESH
 	{
 		for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
 		{
-			m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
+			m_doubleDispatchContactPoints[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
-			btAssert(m_doubleDispatch[i][j]);
+			btAssert(m_doubleDispatchContactPoints[i][j]);
+			m_doubleDispatchClosestPoints[i][j] = m_collisionConfiguration->getClosestPointsAlgorithmCreateFunc(i, j);
+
 		}
 	}
 	
@@ -61,7 +63,12 @@ m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESH
 
 void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
 {
-	m_doubleDispatch[proxyType0][proxyType1] = createFunc;
+	m_doubleDispatchContactPoints[proxyType0][proxyType1] = createFunc;
+}
+
+void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc)
+{
+	m_doubleDispatchClosestPoints[proxyType0][proxyType1] = createFunc;
 }
 
 btCollisionDispatcher::~btCollisionDispatcher()
@@ -138,14 +145,23 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
 
 	
 
-btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold)
+
+btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType)
 {
 	
 	btCollisionAlgorithmConstructionInfo ci;
 
 	ci.m_dispatcher1 = this;
 	ci.m_manifold = sharedManifold;
-	btCollisionAlgorithm* algo = m_doubleDispatch[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0Wrap,body1Wrap);
+	btCollisionAlgorithm* algo = 0;
+	if (algoType == BT_CONTACT_POINT_ALGORITHMS)
+	{
+		algo = m_doubleDispatchContactPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap);
+	}
+	else
+	{
+		algo = m_doubleDispatchClosestPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap);
+	}
 
 	return algo;
 }
@@ -262,7 +278,7 @@ void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair,
 			//dispatcher will keep algorithms persistent in the collision pair
 			if (!collisionPair.m_algorithm)
 			{
-				collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap);
+				collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap,0, BT_CONTACT_POINT_ALGORITHMS);
 			}
 
 			if (collisionPair.m_algorithm)

src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h

@@ -57,7 +57,9 @@ protected:
 
 	btPoolAllocator*	m_persistentManifoldPoolAllocator;
 
-	btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
+	btCollisionAlgorithmCreateFunc* m_doubleDispatchContactPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
+
+	btCollisionAlgorithmCreateFunc* m_doubleDispatchClosestPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
 
 	btCollisionConfiguration*	m_collisionConfiguration;
 
@@ -84,6 +86,8 @@ public:
 	///registerCollisionCreateFunc allows registration of custom/alternative collision create functions
 	void	registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc);
 
+	void	registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc);
+
 	int	getNumManifolds() const
 	{ 
 		return int( m_manifoldsPtr.size());
@@ -115,7 +119,7 @@ public:
 
 	virtual void clearManifold(btPersistentManifold* manifold);
 
-	btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold = 0);
+	btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType);
 		
 	virtual bool	needsCollision(const btCollisionObject* body0,const btCollisionObject* body1);
 	

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -1231,7 +1231,7 @@ struct btSingleContactCallback : public btBroadphaseAabbCallback
 			btCollisionObjectWrapper ob0(0,m_collisionObject->getCollisionShape(),m_collisionObject,m_collisionObject->getWorldTransform(),-1,-1);
 			btCollisionObjectWrapper ob1(0,collisionObject->getCollisionShape(),collisionObject,collisionObject->getWorldTransform(),-1,-1);
 
-			btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1);
+			btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1,0, BT_CLOSEST_POINT_ALGORITHMS);
 			if (algorithm)
 			{
 				btBridgedManifoldResult contactPointResult(&ob0,&ob1, m_resultCallback);
@@ -1267,7 +1267,7 @@ void	btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionOb
 	btCollisionObjectWrapper obA(0,colObjA->getCollisionShape(),colObjA,colObjA->getWorldTransform(),-1,-1);
 	btCollisionObjectWrapper obB(0,colObjB->getCollisionShape(),colObjB,colObjB->getWorldTransform(),-1,-1);
 
-	btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB);
+	btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB, 0, BT_CLOSEST_POINT_ALGORITHMS);
 	if (algorithm)
 	{
 		btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback);

src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

@@ -65,7 +65,13 @@ void	btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionO
 			const btCollisionShape* childShape = compoundShape->getChildShape(i);
 
 			btCollisionObjectWrapper childWrap(colObjWrap,childShape,colObjWrap->getCollisionObject(),colObjWrap->getWorldTransform(),-1,i);//wrong child trans, but unused (hopefully)
-			m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold);
+			m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
+
+
+			btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsContact;
+			btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsClosestPoints;
+
+
 		}
 	}
 }
@@ -128,8 +134,14 @@ public:
 		btTransform	newChildWorldTrans = orgTrans*childTrans ;
 
 		//perform an AABB check first
-		btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1;
+		btVector3 aabbMin0,aabbMax0;
 		childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0);
+
+		btVector3 extendAabb(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
+		aabbMin0 -= extendAabb;
+		aabbMax0 += extendAabb;
+
+		btVector3 aabbMin1, aabbMax1;
 		m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1);
 
 		if (gCompoundChildShapePairCallback)
@@ -142,12 +154,22 @@ public:
 		{
 
 			btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index);
+			
+			btCollisionAlgorithm* algo = 0;
 
-
+			if (m_resultOut->m_closestPointDistanceThreshold > 0)
-			//the contactpoint is still projected back using the original inverted worldtrans
+			{
-			if (!m_childCollisionAlgorithms[index])
+				algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
-				m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap,m_otherObjWrap,m_sharedManifold);
+			}
-
+			else
+			{
+				//the contactpoint is still projected back using the original inverted worldtrans
+				if (!m_childCollisionAlgorithms[index])
+				{
+					m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
+				}
+				algo = m_childCollisionAlgorithms[index];
+			}
 			
 			const btCollisionObjectWrapper* tmpWrap = 0;
 
@@ -164,8 +186,7 @@ public:
 				m_resultOut->setShapeIdentifiersB(-1,index);
 			}
 
-
+			algo->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut);
-			m_childCollisionAlgorithms[index]->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut);
 
 #if 0
 			if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb))

src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp

@@ -164,9 +164,7 @@ struct	btCompoundCompoundLeafCallback : btDbvt::ICollide
 		btVector3 thresholdVec(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold);
 
 		aabbMin0 -= thresholdVec;
-		aabbMin1 -= thresholdVec;
 		aabbMax0 += thresholdVec;
-		aabbMax1 += thresholdVec;
 
 		if (gCompoundCompoundChildShapePairCallback)
 		{
@@ -183,17 +181,24 @@ struct	btCompoundCompoundLeafCallback : btDbvt::ICollide
 			btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1);
 
 			btCollisionAlgorithm* colAlgo = 0;
+			if (m_resultOut->m_closestPointDistanceThreshold > 0)
+			{
+				colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS);
+			}
+			else
+			{
+				if (pair)
+				{
+					colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
 
-			if (pair)
+				}
-			{
+				else
-				colAlgo = (btCollisionAlgorithm*)pair->m_userPointer;
+				{
-				
+					colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS);
-			} else
+					pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0, childIndex1);
-			{
+					btAssert(pair);
-				colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold);
+					pair->m_userPointer = colAlgo;
-				pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1);
+				}
-				btAssert(pair);
-				pair->m_userPointer = colAlgo;
 			}
 
 			btAssert(colAlgo);

src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

@@ -118,8 +118,16 @@ partId, int triangleIndex)
 		
 		
 		btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform?
-		btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap,&triObWrap,m_manifoldPtr);
+		btCollisionAlgorithm* colAlgo = 0;
-
+		
+		if (m_resultOut->m_closestPointDistanceThreshold > 0)
+		{
+			colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
+		}
+		else
+		{
+			colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, m_manifoldPtr, BT_CONTACT_POINT_ALGORITHMS);
+		}
 		const btCollisionObjectWrapper* tmpWrap = 0;
 
 		if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
@@ -170,7 +178,8 @@ void	btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr
 	const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
 	//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
 	convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
-	btScalar extraMargin = collisionMarginTriangle;
+	btScalar extraMargin = collisionMarginTriangle+ resultOut->m_closestPointDistanceThreshold;
+	
 	btVector3 extra(extraMargin,extraMargin,extraMargin);
 
 	m_aabbMax += extra;

src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp

@@ -198,6 +198,86 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration()
 
 }
 
+btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1)
+{
+
+
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
+	{
+		return	m_sphereSphereCF;
+	}
+#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE))
+	{
+		return	m_sphereBoxCF;
+	}
+
+	if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
+	{
+		return	m_boxSphereCF;
+	}
+#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM
+
+
+	if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE))
+	{
+		return	m_sphereTriangleCF;
+	}
+
+	if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE))
+	{
+		return	m_triangleSphereCF;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE))
+	{
+		return m_convexPlaneCF;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE))
+	{
+		return m_planeConvexCF;
+	}
+
+
+
+	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1))
+	{
+		return m_convexConvexCreateFunc;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1))
+	{
+		return m_convexConcaveCreateFunc;
+	}
+
+	if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0))
+	{
+		return m_swappedConvexConcaveCreateFunc;
+	}
+
+
+	if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1))
+	{
+		return m_compoundCompoundCreateFunc;
+	}
+
+	if (btBroadphaseProxy::isCompound(proxyType0))
+	{
+		return m_compoundCreateFunc;
+	}
+	else
+	{
+		if (btBroadphaseProxy::isCompound(proxyType1))
+		{
+			return m_swappedCompoundCreateFunc;
+		}
+	}
+
+	//failed to find an algorithm
+	return m_emptyCreateFunc;
+
+}
 
 btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1)
 {

src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h

@@ -103,6 +103,8 @@ public:
 
 	virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);
 
+	virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1);
+
 	///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm.
 	///By default, this feature is disabled for best performance.
 	///@param numPerturbationIterations controls the number of collision queries. Set it to zero to disable the feature.

src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h

@@ -122,7 +122,7 @@ protected:
 		checkManifold(body0Wrap,body1Wrap);
 
 		btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm(
-				body0Wrap,body1Wrap,getLastManifold());
+				body0Wrap,body1Wrap,getLastManifold(), BT_CONTACT_POINT_ALGORITHMS);
 		return convex_algorithm ;
 	}
 

src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp

@@ -120,8 +120,8 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId,
 		btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1);
 		//btCollisionObjectWrapper triBody(0,tm, ob, btTransform::getIdentity());//ob->getWorldTransform());//??
 		btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);
-
+		ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS;
-		btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr);
+		btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0, algoType);//m_manifoldPtr);
 
 		colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut);
 		colAlgo->~btCollisionAlgorithm();
@@ -164,7 +164,8 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId,
 		btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1);
 		btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);//btTransform::getIdentity());//??
 
-		btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr);
+		ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS;
+		btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0, algoType);//m_manifoldPtr);
 
 		colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut);
 		colAlgo->~btCollisionAlgorithm();