Added method to collect all persistent contact manifolds, given a btBroadphasePair (collision algorithm)

virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
See Demos/CharacterDemo/CharacterDemo.cpp for example usage of getAllContactManifolds
Added btDbvtBroadphase to btBulletCollisionCommon.h headerfile
Enable soft body vs concave (albeit slow)
Improved contact point debug rendering (moved from constraint solver into debugDrawWorld)

Demos/AllBulletDemos/Main.cpp

@@ -50,7 +50,7 @@ namespace
 	float viewY = 0.0f;
 	int tx, ty, tw, th;
 	int	gDrawAabb=0;
-	int	gDebugDraw=0;
+	int	gWireFrame=0;
 	int	gDebugContacts=0;
 	int gDebugNoDeactivation = 0;
 }
@@ -120,7 +120,7 @@ void SimulationLoop()
 	{
 		demo->setDebugMode(demo->getDebugMode() & (~btIDebugDraw::DBG_DrawAabb));
 	}
-	if (gDebugDraw)
+	if (gWireFrame)
 	{
 		demo->setDebugMode(demo->getDebugMode() |btIDebugDraw::DBG_DrawWireframe);
 	} else
@@ -284,7 +284,7 @@ int main(int argc, char** argv)
 	GLUI_Panel* drawPanel =	glui->add_panel("Draw");
 
 	glui->add_checkbox_to_panel(drawPanel, "AABBs", &gDrawAabb);
-	glui->add_checkbox_to_panel(drawPanel, "DebugDrawer", &gDebugDraw);
+	glui->add_checkbox_to_panel(drawPanel, "Wireframe", &gWireFrame);
 	glui->add_checkbox_to_panel(drawPanel, "Contacts", &gDebugContacts);
 
 //	glui->add_checkbox_to_panel(drawPanel, "Impulses", &settings.drawImpulses);

Demos/Benchmarks/BenchmarkDemo.cpp

@@ -275,7 +275,6 @@ void	BenchmarkDemo::initPhysics()
 	m_overlappingPairCache = new btAxisSweep3(worldAabbMin,worldAabbMax,3500);
 	//m_overlappingPairCache = new btDbvtBroadphase();
 
-
 	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
 	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
 	sol->setSolverMode(btSequentialImpulseConstraintSolver::SOLVER_CACHE_FRIENDLY);

Demos/BspDemo/BspDemo.cpp

@@ -147,7 +147,8 @@ void	BspDemo::initPhysics(char* bspfilename)
 	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
 	btVector3 worldMin(-1000,-1000,-1000);
 	btVector3 worldMax(1000,1000,1000);
-	m_broadphase = new btAxisSweep3(worldMin,worldMax);
+	m_broadphase = new btDbvtBroadphase();
+	//m_broadphase = new btAxisSweep3(worldMin,worldMax);
 	//btOverlappingPairCache* broadphase = new btSimpleBroadphase();
 	m_solver = new btSequentialImpulseConstraintSolver();
 	//ConstraintSolver* solver = new OdeConstraintSolver;
@@ -235,6 +236,9 @@ void BspDemo::displayCallback(void) {
 
 	renderme();
 
+	//optional but useful: debug drawing
+	if (m_dynamicsWorld)
+		m_dynamicsWorld->debugDrawWorld();
 
 	glFlush();
 	glutSwapBuffers();

Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp

@@ -295,9 +295,12 @@ void CcdPhysicsDemo::displayCallback(void) {
 
 	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
 
-	
 	renderme();
 
+	//optional but useful: debug drawing
+	if (m_dynamicsWorld)
+		m_dynamicsWorld->debugDrawWorld();
+
 	glFlush();
 	glutSwapBuffers();
 }

Demos/CharacterDemo/CharacterDemo.cpp

@@ -97,6 +97,64 @@ CharacterDemo::~CharacterDemo()
 
 }
 
+class MyCustomOverlappingPairCallback : public btOverlappingPairCallback
+{
+
+	CharacterDemo*	m_characterDemo;
+	btCollisionObject*	m_characterCollider;
+
+	btHashedOverlappingPairCache*	m_hashPairCache;
+
+public:
+
+	MyCustomOverlappingPairCallback(CharacterDemo* demo,btCollisionObject* characterCollider)
+		:m_characterDemo(demo),
+		m_characterCollider(characterCollider)
+	{
+		m_hashPairCache = new btHashedOverlappingPairCache();
+	}
+	
+	virtual ~MyCustomOverlappingPairCallback()
+	{
+		delete m_hashPairCache;
+	}
+
+	virtual btBroadphasePair*	addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
+	{
+		if (proxy0->m_clientObject==m_characterCollider || proxy1->m_clientObject==m_characterCollider)
+		{
+			printf("addOverlappingPair (%x,%x)\n",proxy0,proxy1);
+			return m_hashPairCache->addOverlappingPair(proxy0,proxy1);
+		}
+		return 0;
+	}
+
+	virtual void*	removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
+	{
+		if (proxy0->m_clientObject==m_characterCollider || proxy1->m_clientObject==m_characterCollider)
+		{
+			printf("removeOverlappingPair (%x,%x)\n",proxy0,proxy1);
+			return m_hashPairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
+		}
+		return 0;
+	}
+
+	virtual void	removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher)
+	{
+		if (proxy0->m_clientObject==m_characterCollider)
+		{
+			printf("removeOverlappingPairsContainingProxy (%x)\n",proxy0);
+			m_hashPairCache->removeOverlappingPairsContainingProxy(proxy0,dispatcher);
+		}
+	}
+	
+	btBroadphasePairArray&	getOverlappingPairArray()
+	{
+		return m_hashPairCache->getOverlappingPairArray();
+	}
+
+};
+
 void CharacterDemo::initPhysics()
 {
 
@@ -106,7 +164,9 @@ void CharacterDemo::initPhysics()
 	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
 	btVector3 worldMin(-1000,-1000,-1000);
 	btVector3 worldMax(1000,1000,1000);
-	m_overlappingPairCache = new btAxisSweep3(worldMin,worldMax);
+	btAxisSweep3* sweepBP = new btAxisSweep3(worldMin,worldMax);
+	m_overlappingPairCache = sweepBP;
+		
 	m_constraintSolver = new btSequentialImpulseConstraintSolver();
 	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration);
 	//m_dynamicsWorld->setGravity(btVector3(0,0,0));
@@ -240,6 +300,14 @@ const float TRIANGLE_SIZE=20.f;
 	m_character = new CharacterController ();
 	m_character->setup (m_dynamicsWorld);
 
+	//we need to remove the rigid body from the broadphase in order to register all collisions
+	m_dynamicsWorld->removeRigidBody(m_character->getRigidBody());
+	//some custom callback sample
+	m_customPairCallback = new MyCustomOverlappingPairCallback(this,m_character->getRigidBody());
+	sweepBP->setOverlappingPairUserCallback(m_customPairCallback);
+	m_dynamicsWorld->addRigidBody(m_character->getRigidBody());
+
+
 #define CUBE_HALF_EXTENTS 0.5
 #define EXTRA_HEIGHT 10.0
 	btBoxShape* boxShape = new btBoxShape (btVector3(1.0, 1.0, 1.0));
@@ -331,6 +399,34 @@ void CharacterDemo::clientMoveAndDisplay()
 		}
 	}
 
+	printf("numPairs = %d\n",m_customPairCallback->getOverlappingPairArray().size());
+	{
+		btManifoldArray	manifoldArray;
+		for (int i=0;i<m_customPairCallback->getOverlappingPairArray().size();i++)
+		{
+			manifoldArray.clear();
+			
+			const btBroadphasePair& pair = m_customPairCallback->getOverlappingPairArray()[i];
+			btBroadphasePair* collisionPair = m_overlappingPairCache->getOverlappingPairCache()->findPair(pair.m_pProxy0,pair.m_pProxy1);
+
+			if (collisionPair->m_algorithm)
+				collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
+
+			for (int j=0;j<manifoldArray.size();j++)
+			{
+				btPersistentManifold* manifold = manifoldArray[j];
+				for (int p=0;p<manifold->getNumContacts();p++)
+				{
+					const btManifoldPoint&pt = manifold->getContactPoint(p);
+
+					m_dynamicsWorld->getDebugDrawer()->drawContactPoint(pt.getPositionWorldOnB(),pt.m_normalWorldOnB,pt.getDistance(),pt.getLifeTime(),btVector3(1.f,1.f,0.f));
+				}
+			}
+		}
+	}
+
+
+	
 	if (m_dynamicsWorld)
 	{
 		//during idle mode, just run 1 simulation step maximum
@@ -339,9 +435,10 @@ void CharacterDemo::clientMoveAndDisplay()
 			dt = 1.0/420.f;
 
 		int numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps);
-		//optional but useful: debug drawing
-		m_dynamicsWorld->debugDrawWorld();
 		
+		//optional but useful: debug drawing
+		if (m_dynamicsWorld)
+			m_dynamicsWorld->debugDrawWorld();
 
 //#define VERBOSE_FEEDBACK
 #ifdef VERBOSE_FEEDBACK
@@ -393,6 +490,10 @@ void CharacterDemo::displayCallback(void)
 
 	renderme();
 
+	//optional but useful: debug drawing
+	if (m_dynamicsWorld)
+		m_dynamicsWorld->debugDrawWorld();
+
 
 	glFlush();
 	glutSwapBuffers();

Demos/CharacterDemo/CharacterDemo.h

@@ -41,6 +41,8 @@ class CharacterDemo : public DemoApplication
 
 	class btTriangleIndexVertexArray*	m_indexVertexArrays;
 
+	class	MyCustomOverlappingPairCallback*	m_customPairCallback;
+
 	btVector3*	m_vertices;
 
 	

Demos/CharacterDemo/main.cpp

@@ -1,7 +1,10 @@
 
 #include "CharacterDemo.h"
 #include "GlutStuff.h"
+#include "GLDebugDrawer.h"
+#include "btBulletDynamicsCommon.h"
 
+GLDebugDrawer	gDebugDrawer;
 
 int main(int argc,char** argv)
 {
@@ -9,6 +12,7 @@ int main(int argc,char** argv)
         CharacterDemo* characterDemo = new CharacterDemo;
 
         characterDemo->initPhysics(); 
+		characterDemo->getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
 
         return glutmain(argc, argv,640,480,"Bullet Character Demo. http://www.continuousphysics.com/Bullet/phpBB2/", characterDemo);
 }

Demos/ConcaveDemo/ConcavePhysicsDemo.cpp

@@ -352,6 +352,7 @@ void ConcaveDemo::clientMoveAndDisplay()
 	//optional but useful: debug drawing
 	m_dynamicsWorld->debugDrawWorld();
 
+	
 	renderme();
 
     glFlush();
@@ -368,6 +369,11 @@ void ConcaveDemo::displayCallback(void) {
 
 	renderme();
 
+	//optional but useful: debug drawing
+	if (m_dynamicsWorld)
+		m_dynamicsWorld->debugDrawWorld();
+
+
     glFlush();
     glutSwapBuffers();
 }

Demos/GimpactTestDemo/GimpactTestDemo.cpp

@@ -628,6 +628,10 @@ void GimpactConcaveDemo::displayCallback(void) {
 
 	renderme();
 
+	//optional but useful: debug drawing
+	if (m_dynamicsWorld)
+		m_dynamicsWorld->debugDrawWorld();
+
 	glFlush();
 	glutSwapBuffers();
 }

Demos/MovingConcaveDemo/ConcavePhysicsDemo.cpp

@@ -1909,6 +1909,10 @@ void ConcaveDemo::displayCallback(void) {
 
 	renderme();
 
+	//optional but useful: debug drawing
+	if (m_dynamicsWorld)
+		m_dynamicsWorld->debugDrawWorld();
+
     glFlush();
     glutSwapBuffers();
 }

Demos/MultiThreadedDemo/MultiThreadedDemo.cpp

@@ -212,6 +212,10 @@ void MultiThreadedDemo::displayCallback(void) {
 
 	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
 
+	//optional but useful: debug drawing
+	if (m_dynamicsWorld)
+		m_dynamicsWorld->debugDrawWorld();
+
 	
 	renderme();
 

Demos/OpenGL/GLDebugDrawer.cpp

@@ -68,7 +68,7 @@ void	GLDebugDrawer::reportErrorWarning(const char* warningString)
 
 void	GLDebugDrawer::drawContactPoint(const btVector3& pointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)
 {
-	if (m_debugMode & btIDebugDraw::DBG_DrawContactPoints)
+	
 	{
 		btVector3 to=pointOnB+normalOnB*distance;
 		const btVector3&from = pointOnB;

Demos/RagdollDemo/RagdollDemo.cpp

@@ -373,9 +373,11 @@ void RagdollDemo::clientMoveAndDisplay()
 	if (m_dynamicsWorld)
 	{
 		m_dynamicsWorld->stepSimulation(ms / 1000000.f);
+		
 		//optional but useful: debug drawing
 		m_dynamicsWorld->debugDrawWorld();
 
+
 	}
 
 	renderme(); 
@@ -391,6 +393,10 @@ void RagdollDemo::displayCallback()
 
 	renderme();
 
+	//optional but useful: debug drawing
+	if (m_dynamicsWorld)
+		m_dynamicsWorld->debugDrawWorld();
+
 	glFlush();
 	glutSwapBuffers();
 }

Demos/RagdollDemo/main.cpp

@@ -1,13 +1,17 @@
 
 #include "RagdollDemo.h"
 #include "GlutStuff.h"
+#include "GLDebugDrawer.h"
+#include "btBulletDynamicsCommon.h"
+
+GLDebugDrawer	gDebugDrawer;
 
 int main(int argc,char* argv[])
 {
         RagdollDemo demoApp;
 
         demoApp.initPhysics();
-       
+		demoApp.getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
 
         return glutmain(argc, argv,640,480,"Bullet Physics Demo. http://bullet.sf.net",&demoApp);
 }

Demos/SoftDemo/SoftDemo.cpp

@@ -1182,7 +1182,7 @@ void	SoftDemo::initPhysics()
 
 
 	btCollisionShape* groundShape = 0;
-	bool useConcaveMesh = false;//not ready yet true;
+	bool useConcaveMesh = true;//not ready yet true;
 
 	if (useConcaveMesh)
 	{

Demos/VehicleDemo/VehicleDemo.cpp

@@ -450,8 +450,6 @@ void VehicleDemo::clientMoveAndDisplay()
 			dt = 1.0/420.f;
 
 		int numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps);
-		//optional but useful: debug drawing
-		m_dynamicsWorld->debugDrawWorld();
 		
 
 //#define VERBOSE_FEEDBACK
@@ -486,6 +484,10 @@ void VehicleDemo::clientMoveAndDisplay()
 
 	renderme(); 
 
+	//optional but useful: debug drawing
+	if (m_dynamicsWorld)
+		m_dynamicsWorld->debugDrawWorld();
+
 #ifdef USE_QUICKPROF 
         btProfiler::endBlock("render"); 
 #endif 
@@ -504,6 +506,9 @@ void VehicleDemo::displayCallback(void)
 
 	renderme();
 
+//optional but useful: debug drawing
+	if (m_dynamicsWorld)
+		m_dynamicsWorld->debugDrawWorld();
 
 	glFlush();
 	glutSwapBuffers();

Extras/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h

@@ -46,6 +46,12 @@ public:
 
 	virtual ~SpuContactManifoldCollisionAlgorithm();
 
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr)
+			manifoldArray.push_back(m_manifoldPtr);
+	}
+
 	btPersistentManifold*	getContactManifoldPtr()
 	{
 		return m_manifoldPtr;

Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactCollisionAlgorithm.h

@@ -204,7 +204,11 @@ public:
 
 	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
-
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr)
+			manifoldArray.push_back(m_manifoldPtr);
+	}
 
 
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc

src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h

@@ -17,6 +17,7 @@ subject to the following restrictions:
 #define COLLISION_ALGORITHM_H
 
 #include "LinearMath/btScalar.h"
+#include "LinearMath/btAlignedObjectArray.h"
 
 struct btBroadphaseProxy;
 class btDispatcher;
@@ -25,6 +26,7 @@ class btCollisionObject;
 struct btDispatcherInfo;
 class	btPersistentManifold;
 
+typedef btAlignedObjectArray<btPersistentManifold*>	btManifoldArray;
 
 struct btCollisionAlgorithmConstructionInfo
 {
@@ -71,6 +73,7 @@ public:
 
 	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0;
 
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray) = 0;
 };
 
 

src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h

@@ -41,6 +41,15 @@ public:
 
 	virtual ~btBoxBoxCollisionAlgorithm();
 
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+		{
+			manifoldArray.push_back(m_manifoldPtr);
+		}
+	}
+
+
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)

src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h

@@ -44,6 +44,15 @@ public:
 
 	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		int i;
+		for (i=0;i<m_childCollisionAlgorithms.size();i++)
+		{
+			m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
+		}
+	}
+
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)

src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

@@ -37,6 +37,13 @@ btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
 {
 }
 
+void	btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray&	manifoldArray)
+{
+	if (m_btConvexTriangleCallback.m_manifoldPtr)
+	{
+		manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr);
+	}
+}
 
 
 btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher*  dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped):

src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h

@@ -88,6 +88,8 @@ public:
 
 	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray);
+	
 	void	clearCache();
 
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h

@@ -47,6 +47,14 @@ public:
 
 	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		///should we use m_ownManifold to avoid adding duplicates?
+		if (m_manifoldPtr && m_ownManifold)
+			manifoldArray.push_back(m_manifoldPtr);
+	}
+
+
 	void	setLowLevelOfDetail(bool useLowLevel);
 
 

src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h

@@ -42,6 +42,14 @@ public:
 
 	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+		{
+			manifoldArray.push_back(m_manifoldPtr);
+		}
+	}
+
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)

src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h

@@ -34,6 +34,10 @@ public:
 
 	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+	}
+
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)

src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h

@@ -42,6 +42,14 @@ public:
 
 	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+		{
+			manifoldArray.push_back(m_manifoldPtr);
+		}
+	}
+
 	btScalar getSphereDistance( btCollisionObject* boxObj,btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius );
 
 	btScalar getSpherePenetration( btCollisionObject* boxObj, btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax);

src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h

@@ -41,6 +41,13 @@ public:
 
 	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+		{
+			manifoldArray.push_back(m_manifoldPtr);
+		}
+	}
 	
 	virtual ~btSphereSphereCollisionAlgorithm();
 

src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h

@@ -41,6 +41,13 @@ public:
 
 	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+		{
+			manifoldArray.push_back(m_manifoldPtr);
+		}
+	}
 	
 	virtual ~btSphereTriangleCollisionAlgorithm();
 

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -536,7 +536,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 	END_PROFILE("refreshManifolds");
 #endif //FORCE_REFESH_CONTACT_MANIFOLDS
 
-	btVector3 color(0,1,0);
+	
 
 
 
@@ -648,10 +648,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 					
 					btManifoldPoint& cp = manifold->getContactPoint(j);
 					
-					if (debugDrawer)
-						debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
-
-					
 					if (cp.getDistance() <= btScalar(0.))
 					{
 						
@@ -1110,7 +1106,7 @@ void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
 
 		gTotalContactPoints += numpoints;
 
-		btVector3 color(0,1,0);
+		
 		for (int i=0;i<numpoints ;i++)
 		{
 			btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
@@ -1287,16 +1283,12 @@ btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRig
 
 	{
 
-		btVector3 color(0,1,0);
+		
 		{
 			if (cp.getDistance() <= btScalar(0.))
 			{
 
-				if (iter == 0)
-				{
-					if (debugDrawer)
-						debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
-				}
+				
 
 				{
 
@@ -1325,16 +1317,12 @@ btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBo
 
 	{
 
-		btVector3 color(0,1,0);
+		
 		{
 			if (cp.getDistance() <= btScalar(0.))
 			{
 
-				if (iter == 0)
-				{
-					if (debugDrawer)
-						debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
-				}
+				
 
 				{
 
@@ -1363,7 +1351,7 @@ btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,b
 
 	{
 
-		btVector3 color(0,1,0);
+		
 		{
 			
 			if (cp.getDistance() <= btScalar(0.))

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -125,6 +125,27 @@ void	btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
 
 void	btDiscreteDynamicsWorld::debugDrawWorld()
 {
+
+	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
+	{
+		int numManifolds = getDispatcher()->getNumManifolds();
+		btVector3 color(0,1,0);
+		for (int i=0;i<numManifolds;i++)
+		{
+			btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
+			btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
+			btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
+
+			int numContacts = contactManifold->getNumContacts();
+			for (int j=0;j<numContacts;j++)
+			{
+				btManifoldPoint& cp = contactManifold->getContactPoint(j);
+				getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
+			}
+		}
+	}
+
+
 	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
 	{
 		int i;

src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp

@@ -39,6 +39,8 @@ m_btSoftBodyTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped)
 {
 }
 
+
+
 btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm()
 {
 }
@@ -161,7 +163,7 @@ btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);
 //		btVector3 other;
 		btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
 		normal.normalize();
-		normal*= 0.3f;
+		normal*= 1.3f;
 //		other=(triangle[0]+triangle[1]+triangle[2])*0.333333f;
 //		other+=normal*22.f;
 		btVector3	pts[6] = {triangle[0]+normal,

src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h

@@ -88,6 +88,11 @@ public:
 
 	btScalar	calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		//we don't add any manifolds
+	}
+
 	void	clearCache();
 
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc

src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp

@@ -18,7 +18,7 @@ subject to the following restrictions:
 #include "btSoftBodyConcaveCollisionAlgorithm.h"
 #include "btSoftSoftCollisionAlgorithm.h"
 
-//#define ENABLE_SOFTBODY_CONCAVE_COLLISIONS 1
+#define ENABLE_SOFTBODY_CONCAVE_COLLISIONS 1
 
 btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfiguration(btStackAlloc*	stackAlloc,btPoolAllocator*	persistentManifoldPool,btPoolAllocator*	collisionAlgorithmPool)
 :btDefaultCollisionConfiguration(stackAlloc,persistentManifoldPool,collisionAlgorithmPool)

src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h

@@ -47,6 +47,11 @@ public:
 
 	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		//we don't add any manifolds
+	}
+
 	
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{

src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h

@@ -42,6 +42,12 @@ public:
 
 	virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
 
+	virtual	void	getAllContactManifolds(btManifoldArray&	manifoldArray)
+	{
+		if (m_manifoldPtr && m_ownManifold)
+			manifoldArray.push_back(m_manifoldPtr);
+	}
+
 	btSoftSoftCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
 
 	virtual ~btSoftSoftCollisionAlgorithm();

src/btBulletCollisionCommon.h

@@ -51,6 +51,7 @@ subject to the following restrictions:
 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
 #include "BulletCollision/BroadphaseCollision/btAxisSweep3.h"
 #include "BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h"
+#include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h"
 
 ///Math library & Utils
 #include "LinearMath/btQuaternion.h"