From 0bef2e73632032878b3b4705bceb3c401c6a0192 Mon Sep 17 00:00:00 2001
From: "erwin.coumans" <erwin.coumans@08e121b0-ae19-0410-a57b-3be3395fd4fd>
Date: Thu, 31 Mar 2011 21:14:35 +0000
Subject: [PATCH] Fixes in btContinuousConvexCollision, should fix Issue 347

Continuous sweeps do not detect hits unless body0 and body1 penetrate more than allowedCcdPenetration.
This is to allow sliding objects (characters) where otherwise any contact would result in TOI = 0.
If objects penetrate deeper than allowedCcdPenetration at the start of the sweep, a TOI=0 is reported, unless the motion will separate the objects.

Secondly, a reportFailure method is added to CastResults, to handle failures gracefully (in case max iterations of conservative advancement is reached etc)
---
 .../btContinuousConvexCollision.cpp           | 58 +++++--------------
 .../NarrowPhaseCollision/btConvexCast.h       |  2 +-
 2 files changed, 14 insertions(+), 46 deletions(-)

diff --git a/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
index 9ee83e7d5..002b4e5b4 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
+++ b/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
@@ -64,7 +64,6 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 		return false;
 
 
-	btScalar radius = btScalar(0.001);
 
 	btScalar lambda = btScalar(0.);
 	btVector3 v(1,0,0);
@@ -82,14 +81,12 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 	int numIter = 0;
 	//first solution, using GJK
 
-
 	btTransform identityTrans;
 	identityTrans.setIdentity();
 
 	btSphereShape	raySphere(btScalar(0.0));
 	raySphere.setMargin(btScalar(0.));
 
-
 //	result.drawCoordSystem(sphereTr);
 
 	btPointCollector	pointCollector1;
@@ -99,8 +96,6 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 		btGjkPairDetector gjk(m_convexA,m_convexB,m_convexA->getShapeType(),m_convexB->getShapeType(),m_convexA->getMargin(),m_convexB->getMargin(),m_simplexSolver,m_penetrationDepthSolver);		
 		btGjkPairDetector::ClosestPointInput input;
 	
-		//we don't use margins during CCD
-	//	gjk.setIgnoreMargin(true);
 
 		input.m_transformA = fromA;
 		input.m_transformB = fromB;
@@ -113,31 +108,23 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 	if (hasResult)
 	{
 		btScalar dist;
-		dist = pointCollector1.m_distance;
+		dist = pointCollector1.m_distance + result.m_allowedPenetration;
 		n = pointCollector1.m_normalOnBInWorld;
-
 		btScalar projectedLinearVelocity = relLinVel.dot(n);
-		
+		if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
+			return false;
+
 		//not close enough
-		while (dist > radius)
+		while (dist > SIMD_EPSILON)
 		{
 			if (result.m_debugDrawer)
 			{
 				result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1));
 			}
-			numIter++;
-			if (numIter > maxIter)
-			{
-				return false; //todo: report a failure
-			}
 			btScalar dLambda = btScalar(0.);
 
 			projectedLinearVelocity = relLinVel.dot(n);
 
-			//calculate safe moving fraction from distance / (linear+rotational velocity)
-			
-			//btScalar clippedDist  = GEN_min(angularConservativeRadius,dist);
-			//btScalar clippedDist  = dist;
 			
 			//don't report time of impact for motion away from the contact normal (or causes minor penetration)
 			if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
@@ -189,30 +176,23 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 			gjk.getClosestPoints(input,pointCollector,0);
 			if (pointCollector.m_hasResult)
 			{
-				if (pointCollector.m_distance < btScalar(0.))
-				{
-					//degenerate ?!
-					result.m_fraction = lastLambda;
-					n = pointCollector.m_normalOnBInWorld;
-					result.m_normal=n;//.setValue(1,1,1);// = n;
-					result.m_hitPoint = pointCollector.m_pointInWorld;
-					return true;
-				}
+				dist = pointCollector.m_distance+result.m_allowedPenetration;
 				c = pointCollector.m_pointInWorld;		
 				n = pointCollector.m_normalOnBInWorld;
-				dist = pointCollector.m_distance;
 			} else
 			{
-				//??
+				result.reportFailure(-1, numIter);
 				return false;
 			}
-			
 
+			numIter++;
+			if (numIter > maxIter)
+			{
+				result.reportFailure(-2, numIter);
+				return false;
+			}
 		}
 	
-		if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=result.m_allowedPenetration)//SIMD_EPSILON)
-			return false;
-			
 		result.m_fraction = lambda;
 		result.m_normal = n;
 		result.m_hitPoint = c;
@@ -221,16 +201,4 @@ bool	btContinuousConvexCollision::calcTimeOfImpact(
 
 	return false;
 
-/*
-//todo:
-	//if movement away from normal, discard result
-	btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
-	if (result.m_fraction < btScalar(1.))
-	{
-		if (move.dot(result.m_normal) <= btScalar(0.))
-		{
-		}
-	}
-*/
-
 }
diff --git a/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
index b0bce341e..887aaef23 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
+++ b/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
@@ -39,7 +39,7 @@ public:
 				
 		virtual void	DebugDraw(btScalar	fraction) {(void)fraction;}
 		virtual void	drawCoordSystem(const btTransform& trans) {(void)trans;}
-
+		virtual void	reportFailure(int errNo, int numIterations) {(void)errNo;(void)numIterations;}
 		CastResult()
 			:m_fraction(btScalar(BT_LARGE_FLOAT)),
 			m_debugDrawer(0),