Calculate multiple contact points (for convex-convex and convex-plane) when less then 3 points exist in the persistent manifold.

Uses the normal pertubation method, described by Gino van den Bergen:  http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=4&t=288&p=888#p888
Made btRigidBody::getInvInertiaDiagLocal const, thanks to abhikp (http://code.google.com/p/bullet/issues/detail?id=183 )

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -51,6 +51,8 @@ subject to the following restrictions:
 
 btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
 {
+	m_numPertubationIterations = 3;
+	m_minimumPointsPertubationThreshold = 3;
 	m_simplexSolver = simplexSolver;
 	m_pdSolver = pdSolver;
 }
@@ -59,17 +61,19 @@ btConvexConvexAlgorithm::CreateFunc::~CreateFunc()
 { 
 }
 
-btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPertubationIterations, int minimumPointsPertubationThreshold)
 : btActivatingCollisionAlgorithm(ci,body0,body1),
 m_simplexSolver(simplexSolver),
 m_pdSolver(pdSolver),
 m_ownManifold (false),
 m_manifoldPtr(mf),
-m_lowLevelOfDetail(false)
+m_lowLevelOfDetail(false),
 #ifdef USE_SEPDISTANCE_UTIL2
 ,m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
-			  (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc())
+			  (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
 #endif
+m_numPertubationIterations(numPertubationIterations),
+m_minimumPointsPertubationThreshold(minimumPointsPertubationThreshold)
 {
 	(void)body0;
 	(void)body1;
@@ -93,8 +97,35 @@ void	btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
 }
 
 
+struct btPertubedContactResult : public btManifoldResult
+{
+	btManifoldResult* m_originalManifoldResult;
+	btTransform m_transformA;
+	btTransform m_transformB;
 
 
+	btPertubedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB)
+		:m_originalManifoldResult(originalResult),
+		m_transformA(transformA),
+		m_transformB(transformB)
+	{
+	}
+	virtual ~ btPertubedContactResult()
+	{
+	}
+
+	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
+	{
+		const btVector3& worldPointB = pointInWorld;
+		btVector3 worldPointA = worldPointB+normalOnBInWorld*depth;
+		btVector3 localA = m_transformA.invXform(worldPointA);
+		btVector3 localB = m_transformB.invXform(pointInWorld);
+		m_originalManifoldResult->addLocalContactPointInternal(	normalOnBInWorld,localA,localB);
+	}
+
+};
+
+extern btScalar gContactBreakingThreshold;
 
 //
 // Convex-Convex collision algorithm
@@ -110,6 +141,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 	}
 	resultOut->setPersistentManifold(m_manifoldPtr);
 
+	//comment-out next line to test multi-contact generation
+	//resultOut->getPersistentManifold()->clearManifold();
 	
 
 	btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
@@ -146,9 +179,57 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 	input.m_transformB = body1->getWorldTransform();
 
 	gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
-
 	btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold;
 
+	//now pertube directions to get multiple contact points
+	btVector3 v0,v1;
+	btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
+	btPlaneSpace1(sepNormalWorldSpace,v0,v1);
+	//now perform 'm_numPertubationIterations' collision queries with the pertubated collision objects
+	
+	//perform pertubation when more then 'm_minimumPointsPertubationThreshold' points
+	if (resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPertubationThreshold)
+	{
+		
+		int i;
+
+		bool pertubeA = true;
+		const btScalar angleLimit = 0.125f * SIMD_PI;
+		btScalar pertubeAngle;
+		btScalar radiusA = min0->getAngularMotionDisc();
+		btScalar radiusB = min1->getAngularMotionDisc();
+		if (radiusA < radiusB)
+		{
+			pertubeAngle = gContactBreakingThreshold /radiusA;
+			pertubeA = true;
+		} else
+		{
+			pertubeAngle = gContactBreakingThreshold / radiusB;
+			pertubeA = false;
+		}
+		if ( pertubeAngle > angleLimit ) 
+				pertubeAngle = angleLimit;
+
+		for ( i=0;i<m_numPertubationIterations;i++)
+		{
+			btQuaternion pertubeRot(v0,pertubeAngle);
+			btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPertubationIterations));
+			btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
+			if (pertubeA)
+			{
+				input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*pertubeRot*rotq)*body0->getWorldTransform().getBasis());
+			} else
+			{
+				input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*pertubeRot*rotq)*body1->getWorldTransform().getBasis());
+			}
+			btPertubedContactResult pertubedResultOut(resultOut,input.m_transformA,input.m_transformB);
+			gjkPairDetector.getClosestPoints(input,pertubedResultOut,dispatchInfo.m_debugDraw);
+			btScalar curSepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold;
+		}
+	}
+
+	
+
 #ifdef USE_SEPDISTANCE_UTIL2
 	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
 	{

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h

@@ -33,7 +33,9 @@ class btConvexPenetrationDepthSolver;
 ///for certain pairs that have a small size ratio
 ///#define USE_SEPDISTANCE_UTIL2 1
 
-///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations.
+///The convexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations between two convex objects.
+///Multiple contact points are calculated by pertubating the orientation of the smallest object orthogonal to the separating normal.
+///This idea was described by Gino van den Bergen in this forum topic http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=4&t=288&p=888#p888
 class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
 {
 #ifdef USE_SEPDISTANCE_UTIL2
@@ -47,12 +49,17 @@ class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
 	btPersistentManifold*	m_manifoldPtr;
 	bool			m_lowLevelOfDetail;
 	
+	int m_numPertubationIterations;
+	int m_minimumPointsPertubationThreshold;
+
+
 	///cache separating vector to speedup collision detection
 	
 
 public:
 
-	btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
+	btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPertubationIterations, int minimumPointsPertubationThreshold);
+
 
 	virtual ~btConvexConvexAlgorithm();
 
@@ -78,8 +85,11 @@ public:
 
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{
+
 		btConvexPenetrationDepthSolver*		m_pdSolver;
 		btSimplexSolverInterface*			m_simplexSolver;
+		int m_numPertubationIterations;
+		int m_minimumPointsPertubationThreshold;
 
 		CreateFunc(btSimplexSolverInterface*			simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
 		
@@ -88,7 +98,7 @@ public:
 		virtual	btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
 		{
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm));
-			return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver);
+			return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver,m_numPertubationIterations,m_minimumPointsPertubationThreshold);
 		}
 	};
 

src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp

@@ -22,13 +22,13 @@ subject to the following restrictions:
 
 //#include <stdio.h>
 
-btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPertubationIterations,btScalar pertubeAngle)
+btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPertubationIterations,int minimumPointsPertubationThreshold)
 : btCollisionAlgorithm(ci),
 m_ownManifold(false),
 m_manifoldPtr(mf),
 m_isSwapped(isSwapped),
 m_numPertubationIterations(numPertubationIterations),
-m_pertubeAngle(pertubeAngle)
+m_minimumPointsPertubationThreshold(minimumPointsPertubationThreshold)
 {
 	btCollisionObject* convexObj = m_isSwapped? col1 : col0;
 	btCollisionObject* planeObj = m_isSwapped? col0 : col1;
@@ -89,6 +89,7 @@ void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion&
 	}
 }
 
+
 void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
 	(void)dispatchInfo;
@@ -105,23 +106,33 @@ void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0
 	const btVector3& planeNormal = planeShape->getPlaneNormal();
 	const btScalar& planeConstant = planeShape->getPlaneConstant();
 
-
-    btVector3 v0,v1;
-	btPlaneSpace1(planeNormal,v0,v1);
 	//first perform a collision query with the non-pertubated collision objects
 	{
 		btQuaternion rotq(0,0,0,1);
 		collideSingleContact(rotq,body0,body1,dispatchInfo,resultOut);
 	}
 
+	if (resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPertubationThreshold)
+	{
+		btVector3 v0,v1;
+		btPlaneSpace1(planeNormal,v0,v1);
 		//now perform 'm_numPertubationIterations' collision queries with the pertubated collision objects
-	btQuaternion pertubeRot(v0,m_pertubeAngle);
+
+		const btScalar angleLimit = 0.125f * SIMD_PI;
+		btScalar pertubeAngle;
+		btScalar radius = convexShape->getAngularMotionDisc();
+		pertubeAngle = gContactBreakingThreshold / radius;
+		if ( pertubeAngle > angleLimit ) 
+				pertubeAngle = angleLimit;
+
+		btQuaternion pertubeRot(v0,pertubeAngle);
 		for (int i=0;i<m_numPertubationIterations;i++)
 		{
 			btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPertubationIterations));
 			btQuaternion rotq(planeNormal,iterationAngle);
 			collideSingleContact(rotq.inverse()*pertubeRot*rotq,body0,body1,dispatchInfo,resultOut);
 		}
+	}
 
 	if (m_ownManifold)
 	{

src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h

@@ -32,11 +32,11 @@ class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm
 	btPersistentManifold*	m_manifoldPtr;
 	bool		m_isSwapped;
 	int			m_numPertubationIterations;
-	btScalar	m_pertubeAngle;
+	int			m_minimumPointsPertubationThreshold;
 
 public:
 
-	btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPertubationIterations, btScalar pertubeAngle);
+	btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPertubationIterations,int minimumPointsPertubationThreshold);
 
 	virtual ~btConvexPlaneCollisionAlgorithm();
 
@@ -57,11 +57,11 @@ public:
 	struct CreateFunc :public 	btCollisionAlgorithmCreateFunc
 	{
 		int	m_numPertubationIterations;
-		btScalar	m_pertubeAngle;
+		int m_minimumPointsPertubationThreshold;
 			
 		CreateFunc() 
-			: m_numPertubationIterations(10),
+			: m_numPertubationIterations(3),
-			m_pertubeAngle(0.05f)
+			m_minimumPointsPertubationThreshold(3)
 		{
 		}
 		
@@ -70,10 +70,10 @@ public:
 			void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm));
 			if (!m_swapped)
 			{
-				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPertubationIterations,m_pertubeAngle);
+				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPertubationIterations,m_minimumPointsPertubationThreshold);
 			} else
 			{
-				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPertubationIterations,m_pertubeAngle);
+				return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPertubationIterations,m_minimumPointsPertubationThreshold);
 			}
 		}
 	};

src/BulletCollision/CollisionDispatch/btManifoldResult.cpp

@@ -52,6 +52,14 @@ btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* b
 	m_rootTransB = body1->getWorldTransform();
 }
 
+void btManifoldResult::addLocalContactPointInternal(const btVector3& normalOnBInWorld,const btVector3& localPointA,const btVector3& localPointB)
+{
+	btVector3 worldPointA = m_rootTransA( localPointA );
+	btVector3 worldPointB = m_rootTransB( localPointB );
+	btScalar depth = (worldPointA -  worldPointB).dot(normalOnBInWorld);
+
+	addContactPoint(normalOnBInWorld,worldPointB,depth);
+}
 
 void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
 {

src/BulletCollision/CollisionDispatch/btManifoldResult.h

@@ -45,6 +45,8 @@ class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result
 	int m_partId1;
 	int m_index0;
 	int m_index1;
+	
+
 public:
 
 	btManifoldResult()
@@ -77,8 +79,11 @@ public:
 			m_index1=index1;		
 	}
 
+
 	virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth);
 
+	virtual void addLocalContactPointInternal(const btVector3& normalOnBInWorld,const btVector3& localPointA,const btVector3& localPointB);
+
 	SIMD_FORCE_INLINE	void refreshContactPoints()
 	{
 		btAssert(m_manifoldPtr);

src/BulletDynamics/Dynamics/btRigidBody.h

@@ -243,7 +243,7 @@ public:
 		return m_totalTorque;
 	};
     
-	const btVector3& getInvInertiaDiagLocal()
+	const btVector3& getInvInertiaDiagLocal() const
 	{
 		return m_invInertiaLocal;
 	};