fixed a crashing bug in new vehicle physics, and removed some debugging code in contact/friction physics code.

5 files changed, 89 insertions, 94 deletions

diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp
index 99e2bc5e9c7..5264662b519 100644
--- a/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp
+++ b/extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp
@@ -124,14 +124,16 @@ void	CollisionWorld::RemoveCollisionObject(CollisionObject* collisionObject)
 	//bool removeFromBroadphase = false;
 	
 	{
-		/*BroadphaseInterface* scene = */GetBroadphase();
-		BroadphaseProxy* bp = collisionObject->m_broadphaseHandle;
 		
-		//
-		// only clear the cached algorithms
-		//
-		GetBroadphase()->CleanProxyFromPairs(bp);
-		GetBroadphase()->DestroyProxy(bp);
+		BroadphaseProxy* bp = collisionObject->m_broadphaseHandle;
+		if (bp)
+		{
+			//
+			// only clear the cached algorithms
+			//
+			GetBroadphase()->CleanProxyFromPairs(bp);
+			GetBroadphase()->DestroyProxy(bp);
+		}
 	}
 
 
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp
index d482be27659..23b92c63792 100644
--- a/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp
@@ -128,7 +128,7 @@ bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simpl
 			
 		}
 	}
-
+//#define DEBUG_DRAW 1
 #ifdef DEBUG_DRAW
 	if (debugDraw)
 	{
@@ -165,7 +165,9 @@ bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simpl
 	MyResult res;
 	gjkdet.GetClosestPoints(input,res,debugDraw);
 
-	
+	//the penetration depth is over-estimated, relax it
+	float penetration_relaxation= 0.1f;
+	minNorm*=penetration_relaxation;
 
 	if (res.m_hasResult)
 	{
@@ -185,3 +187,6 @@ bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simpl
 	}
 	return res.m_hasResult;
 }
+
+
+
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp
index 38c82fdb07d..e8e8d37f6d6 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp
@@ -123,18 +123,6 @@ float resolveSingleCollision(
 	
 //	printf("distance=%f\n",distance);
 
-	if (distance>0.f)
-	{
-		contactPoint.m_appliedImpulse = 0.f;
-		contactPoint.m_accumulatedTangentImpulse0 = 0.f;
-		contactPoint.m_accumulatedTangentImpulse1 = 0.f;
-		
-		return 0.f;
-	} 
-
-#define MAXPENETRATIONPERFRAME -0.05
-	distance = distance < MAXPENETRATIONPERFRAME? MAXPENETRATIONPERFRAME:distance;
-
 	const SimdVector3& normal = contactPoint.m_normalWorldOnB;
 
 	SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
@@ -153,7 +141,7 @@ float resolveSingleCollision(
 	SimdScalar Kfps = 1.f / solverInfo.m_timeStep ;
 
 	float damping = solverInfo.m_damping ;
-	float Kerp = solverInfo.m_tau;
+	float Kerp = solverInfo.m_erp;
 	
 	if (useGlobalSettingContacts)
 	{
@@ -168,7 +156,7 @@ float resolveSingleCollision(
 	float clipDist = distance + allowedPenetration;
 	float dist = (clipDist > 0.f) ? 0.f : clipDist;
 	//distance = 0.f;
-	SimdScalar positionalError = Kcor *-clipDist;
+	SimdScalar positionalError = Kcor *-dist;
 	//jacDiagABInv;
 	SimdScalar velocityError = -(1.0f + restitution) * rel_vel;// * damping;
 
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h b/extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h
index ec006dab64b..6c6325df8a6 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h
@@ -23,11 +23,12 @@ struct ContactSolverInfo
 	inline ContactSolverInfo()
 	{
 		m_tau = 0.4f;
-		m_damping = 0.9f;
+		m_damping = 1.0f;
 		m_friction = 0.3f;
 		m_restitution = 0.f;
 		m_maxErrorReduction = 20.f;
 		m_numIterations = 10;
+		m_erp = 0.4f;
 		m_sor = 1.3f;
 	}
 
@@ -39,6 +40,7 @@ struct ContactSolverInfo
 	int		m_numIterations;
 	float	m_maxErrorReduction;
 	float	m_sor;
+	float	m_erp;
 
 };
 
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp
index 7fd42d04775..acfca631b72 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp
@@ -26,13 +26,6 @@ subject to the following restrictions:
 #include "JacobianEntry.h"
 #include "GEN_MinMax.h"
 
-//debugging
-bool doApplyImpulse = true;
-
-
-
-bool useImpulseFriction = true;//true;//false;
-
 
 
 
@@ -97,57 +90,59 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta
 		for (int i=0;i<numpoints ;i++)
 		{
 			ManifoldPoint& cp = manifoldPtr->GetContactPoint(i);
-			const SimdVector3& pos1 = cp.GetPositionWorldOnA();
-			const SimdVector3& pos2 = cp.GetPositionWorldOnB();
-
-			SimdVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); 
-			SimdVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
-			
-			//this jacobian entry is re-used for all iterations
-			JacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
-				body1->getCenterOfMassTransform().getBasis().transpose(),
-				rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
-				body1->getInvInertiaDiagLocal(),body1->getInvMass());
+			if (cp.GetDistance() <= 0.f)
+			{
+				const SimdVector3& pos1 = cp.GetPositionWorldOnA();
+				const SimdVector3& pos2 = cp.GetPositionWorldOnB();
 
-			SimdScalar jacDiagAB = jac.getDiagonal();
-			
-			cp.m_jacDiagABInv = 1.f / jacDiagAB;
+				SimdVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); 
+				SimdVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
+				
+				//this jacobian entry is re-used for all iterations
+				JacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
+					body1->getCenterOfMassTransform().getBasis().transpose(),
+					rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
+					body1->getInvInertiaDiagLocal(),body1->getInvMass());
 
-			//for friction
-			cp.m_prevAppliedImpulse = cp.m_appliedImpulse;
+				SimdScalar jacDiagAB = jac.getDiagonal();
+				
+				cp.m_jacDiagABInv = 1.f / jacDiagAB;
 
-			float relaxation = info.m_damping;
-			cp.m_appliedImpulse *= relaxation;
-			
-			//re-calculate friction direction every frame, todo: check if this is really needed
-			SimdPlaneSpace1(cp.m_normalWorldOnB,cp.m_frictionWorldTangential0,cp.m_frictionWorldTangential1);
-
-#ifdef NO_FRICTION_WARMSTART
-			cp.m_accumulatedTangentImpulse0 = 0.f;
-			cp.m_accumulatedTangentImpulse1 = 0.f;
-#endif //NO_FRICTION_WARMSTART
-			float denom0 = body0->ComputeImpulseDenominator(pos1,cp.m_frictionWorldTangential0);
-			float denom1 = body1->ComputeImpulseDenominator(pos2,cp.m_frictionWorldTangential0);
-			float denom = relaxation/(denom0+denom1);
-			cp.m_jacDiagABInvTangent0 = denom;
-
-
-			denom0 = body0->ComputeImpulseDenominator(pos1,cp.m_frictionWorldTangential1);
-			denom1 = body1->ComputeImpulseDenominator(pos2,cp.m_frictionWorldTangential1);
-			denom = relaxation/(denom0+denom1);
-			cp.m_jacDiagABInvTangent1 = denom;
-
-			SimdVector3 totalImpulse = 
-#ifndef NO_FRICTION_WARMSTART
-				cp.m_frictionWorldTangential0*cp.m_accumulatedTangentImpulse0+
-				cp.m_frictionWorldTangential1*cp.m_accumulatedTangentImpulse1+
-#endif //NO_FRICTION_WARMSTART
-				cp.m_normalWorldOnB*cp.m_appliedImpulse;
-
-			//apply previous frames impulse on both bodies
-			body0->applyImpulse(totalImpulse, rel_pos1);
-			body1->applyImpulse(-totalImpulse, rel_pos2);
+				//for friction
+				cp.m_prevAppliedImpulse = cp.m_appliedImpulse;
 
+				float relaxation = info.m_damping;
+				cp.m_appliedImpulse *= relaxation;
+				
+				//re-calculate friction direction every frame, todo: check if this is really needed
+				SimdPlaneSpace1(cp.m_normalWorldOnB,cp.m_frictionWorldTangential0,cp.m_frictionWorldTangential1);
+
+	#ifdef NO_FRICTION_WARMSTART
+				cp.m_accumulatedTangentImpulse0 = 0.f;
+				cp.m_accumulatedTangentImpulse1 = 0.f;
+	#endif //NO_FRICTION_WARMSTART
+				float denom0 = body0->ComputeImpulseDenominator(pos1,cp.m_frictionWorldTangential0);
+				float denom1 = body1->ComputeImpulseDenominator(pos2,cp.m_frictionWorldTangential0);
+				float denom = relaxation/(denom0+denom1);
+				cp.m_jacDiagABInvTangent0 = denom;
+
+
+				denom0 = body0->ComputeImpulseDenominator(pos1,cp.m_frictionWorldTangential1);
+				denom1 = body1->ComputeImpulseDenominator(pos2,cp.m_frictionWorldTangential1);
+				denom = relaxation/(denom0+denom1);
+				cp.m_jacDiagABInvTangent1 = denom;
+
+				SimdVector3 totalImpulse = 
+	#ifndef NO_FRICTION_WARMSTART
+					cp.m_frictionWorldTangential0*cp.m_accumulatedTangentImpulse0+
+					cp.m_frictionWorldTangential1*cp.m_accumulatedTangentImpulse1+
+	#endif //NO_FRICTION_WARMSTART
+					cp.m_normalWorldOnB*cp.m_appliedImpulse;
+
+				//apply previous frames impulse on both bodies
+				body0->applyImpulse(totalImpulse, rel_pos1);
+				body1->applyImpulse(-totalImpulse, rel_pos2);
+			}
 			
 		}
 	}
@@ -166,26 +161,29 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta
 				j=i;
 
 			ManifoldPoint& cp = manifoldPtr->GetContactPoint(j);
-			
-			if (iter == 0)
+			if (cp.GetDistance() <= 0.f)
 			{
-				if (debugDrawer)
-					debugDrawer->DrawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.GetDistance(),cp.GetLifeTime(),color);
-			}
 
-			{
+				if (iter == 0)
+				{
+					if (debugDrawer)
+						debugDrawer->DrawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.GetDistance(),cp.GetLifeTime(),color);
+				}
 
+				{
 
-				//float dist =  cp.GetDistance();
-				//printf("dist(%i)=%f\n",j,dist);
-				float impulse = resolveSingleCollision(
-					*body0,*body1,
-					cp,
-					info);
-				
-				if (maxImpulse < impulse)
-					maxImpulse  = impulse;
 
+					//float dist =  cp.GetDistance();
+					//printf("dist(%i)=%f\n",j,dist);
+					float impulse = resolveSingleCollision(
+						*body0,*body1,
+						cp,
+						info);
+					
+					if (maxImpulse < impulse)
+						maxImpulse  = impulse;
+
+				}
 			}
 		}
 	}
@@ -208,7 +206,7 @@ float SimpleConstraintSolver::SolveFriction(PersistentManifold* manifoldPtr, con
 			int j=i;
 
 			ManifoldPoint& cp = manifoldPtr->GetContactPoint(j);
-
+			if (cp.GetDistance() <= 0.f)
 			{
 
 				resolveSingleFriction(