Added CCD motion camping, to avoid tunneling.

Not as versatile compared to btContinuousDynamicsWorld but still useful.

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");
@@ -720,8 +787,28 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 		if (body)
 		{
 			if (body->isActive() && (!body->isStaticOrKinematicObject()))
+			{
+				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);
 			}
 		}

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();