From 60fa900995590fcea7374037a5c2c442e0cea674 Mon Sep 17 00:00:00 2001
From: "erwin.coumans" <erwin.coumans@08e121b0-ae19-0410-a57b-3be3395fd4fd>
Date: Thu, 11 Sep 2008 18:13:56 +0000
Subject: [PATCH] Added CCD motion camping, to avoid tunneling. Not as
 versatile compared to btContinuousDynamicsWorld but still useful.

---
 .../Dynamics/btDiscreteDynamicsWorld.cpp      | 89 ++++++++++++++++++-
 .../Dynamics/btDiscreteDynamicsWorld.h        |  2 +-
 2 files changed, 89 insertions(+), 2 deletions(-)

diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index 744ed4b88..b0bcbbd7f 100644
--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -707,8 +707,75 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
 	
 }
 
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+
+class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
+{
+	btCollisionObject* m_me;
+	btScalar m_allowedPenetration;
+	btOverlappingPairCache* m_pairCache;
 
 
+public:
+	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) : 
+	  btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
+		m_allowedPenetration(0.0f),
+		m_me(me),
+		m_pairCache(pairCache)
+	{
+	}
+
+	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
+	{
+		if (convexResult.m_hitCollisionObject == m_me)
+			return 1.0;
+
+		btVector3 linVelA,linVelB;
+		linVelA = m_convexToWorld-m_convexFromWorld;
+		linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin();
+
+		btVector3 relativeVelocity = (linVelA-linVelB);
+		//don't report time of impact for motion away from the contact normal (or causes minor penetration)
+		if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration)
+			return 1.f;
+
+		return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
+	}
+
+	virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+	{
+		//don't collide with itself
+		if (proxy0->m_clientObject == m_me)
+			return false;
+
+		///don't do CCD when the collision filters are not matching
+		if (!btCollisionWorld::ClosestConvexResultCallback::needsCollision(proxy0))
+			return false;
+
+		///don't do CCD when there are already contact points (touching contact/penetration)
+		btAlignedObjectArray<btPersistentManifold*> manifoldArray;
+		btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
+		if (collisionPair)
+		{
+			if (collisionPair->m_algorithm)
+			{
+				manifoldArray.resize(0);
+				collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
+				for (int j=0;j<manifoldArray.size();j++)
+				{
+					btPersistentManifold* manifold = manifoldArray[j];
+					if (manifold->getNumContacts()>0)
+						return false;
+				}
+			}
+		}
+		return true;
+	}
+
+
+};
+
+//#include "stdio.h"
 void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
 	BT_PROFILE("integrateTransforms");
@@ -721,7 +788,27 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 		{
 			if (body->isActive() && (!body->isStaticOrKinematicObject()))
 			{
-				body->predictIntegratedTransform(timeStep, predictedTrans);
+				btScalar fraction = 1.f;
+				btScalar squareVel =  body->getLinearVelocity().length2();
+
+				if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareVel)
+				{
+					BT_PROFILE("CCD motion clamping");
+					if (body->getCollisionShape()->isConvex())
+					{
+						body->predictIntegratedTransform(timeStep, predictedTrans);
+						btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache());
+						btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+						btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+						convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
+						if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
+						{
+							fraction = sweepResults.m_closestHitFraction;
+//							printf("clamped integration to hit fraction = %f\n",fraction);
+						}
+					}
+				}
+				body->predictIntegratedTransform(timeStep*fraction, predictedTrans);
 				body->proceedToTransform( predictedTrans);
 			}
 		}
diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
index dcebc89a9..5ed91fac0 100644
--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
+++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
@@ -59,7 +59,7 @@ protected:
 
 	virtual void	predictUnconstraintMotion(btScalar timeStep);
 	
-	void	integrateTransforms(btScalar timeStep);
+	virtual void	integrateTransforms(btScalar timeStep);
 		
 	void	calculateSimulationIslands();