some more improvements to support basic CCD motion clamping

Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp

@@ -556,7 +556,7 @@ int maxNumOutstandingTasks = 4;
 
 
 		// Only do CCD if  motion in one timestep (1.f/60.f) exceeds CUBE_HALF_EXTENTS
-		body->setCcdSquareMotionThreshold( CUBE_HALF_EXTENTS );
+		body->setCcdMotionThreshold( CUBE_HALF_EXTENTS );
 
 		//Experimental: better estimation of CCD Time of Impact:
 		body->setCcdSweptSphereRadius( 0.2*CUBE_HALF_EXTENTS );

src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -31,7 +31,7 @@ btCollisionObject::btCollisionObject()
 		m_internalType(CO_COLLISION_OBJECT),
 		m_hitFraction(btScalar(1.)),
 		m_ccdSweptSphereRadius(btScalar(0.)),
-		m_ccdSquareMotionThreshold(btScalar(0.)),
+		m_ccdMotionThreshold(btScalar(0.)),
 		m_checkCollideWith(false)
 {
 	

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -81,8 +81,8 @@ protected:
 	///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
 	btScalar		m_ccdSweptSphereRadius;
 
-	/// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold
+	/// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold
-	btScalar		m_ccdSquareMotionThreshold;
+	btScalar		m_ccdMotionThreshold;
 	
 	/// If some object should have elaborate collision filtering by sub-classes
 	bool			m_checkCollideWith;
@@ -332,16 +332,22 @@ public:
 		m_ccdSweptSphereRadius = radius;
 	}
 
+	btScalar 	getCcdMotionThreshold() const
+	{
+		return m_ccdMotionThreshold;
+	}
+
 	btScalar 	getCcdSquareMotionThreshold() const
 	{
-		return m_ccdSquareMotionThreshold;
+		return m_ccdMotionThreshold*m_ccdMotionThreshold;
 	}
 
 
-	/// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold
+
-	void	setCcdSquareMotionThreshold(btScalar ccdSquareMotionThreshold)
+	/// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold
+	void	setCcdMotionThreshold(btScalar ccdMotionThreshold)
 	{
-		m_ccdSquareMotionThreshold = ccdSquareMotionThreshold;
+		m_ccdMotionThreshold = ccdMotionThreshold*ccdMotionThreshold;
 	}
 
 	///users can point to their objects, userPointer is not used by Bullet

src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h

@@ -51,7 +51,8 @@ public:
 		int i;
 		for (i=0;i<m_childCollisionAlgorithms.size();i++)
 		{
-			m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
+			if (m_childCollisionAlgorithms[i])
+				m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
 		}
 	}
 

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -269,7 +269,7 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 				{
 					btTransform interpolatedTransform;
 					btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
-						body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform);
+						body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform);
 					body->getMotionState()->setWorldTransform(interpolatedTransform);
 				}
 			}
@@ -790,30 +790,34 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 		btRigidBody* body = btRigidBody::upcast(colObj);
 		if (body)
 		{
+			body->setHitFraction(1.f);
+
 			if (body->isActive() && (!body->isStaticOrKinematicObject()))
 			{
-				btScalar fraction = 1.f;
+				body->predictIntegratedTransform(timeStep, predictedTrans);
-				btScalar squareVel =  body->getLinearVelocity().length2();
+				btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
 
-				if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareVel)
+				if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
 				{
 					BT_PROFILE("CCD motion clamping");
 					if (body->getCollisionShape()->isConvex())
 					{
 						gNumClampedCcdMotions++;
-						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;
+							body->setHitFraction(sweepResults.m_closestHitFraction);
+							body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
+							body->setHitFraction(0.f);
 //							printf("clamped integration to hit fraction = %f\n",fraction);
 						}
 					}
 				}
-				body->predictIntegratedTransform(timeStep*fraction, predictedTrans);
+				
 				body->proceedToTransform( predictedTrans);
 			}
 		}