update Bullet 2.x with latest changes, notice that the integration is not finished yet, and GameBlender is still using extern/bullet.

9 files changed, 391 insertions, 74 deletions

diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
index 9019035f586..429ad54d517 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
@@ -24,16 +24,7 @@ subject to the following restrictions:
 
 #define ASSERT2 assert
 
-//some values to find stable tresholds
-
-float useGlobalSettingContacts = false;//true;
-btScalar contactDamping = 0.2f;
-btScalar contactTau = .02f;//0.02f;//*0.02f;
-
-
-
-
-
+#define USE_INTERNAL_APPLY_IMPULSE 1
 
 
 //bilateral constraint between two dynamic objects
@@ -75,7 +66,9 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
 
 
 	rel_vel = normal.dot(vel);
-		
+	
+	//todo: move this into proper structure
+	btScalar contactDamping = 0.2f;
 
 #ifdef ONLY_USE_LINEAR_MASS
 	btScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
@@ -88,25 +81,17 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
 
 
 
-
-//velocity + friction
 //response  between two dynamic objects with friction
 float resolveSingleCollision(
 	btRigidBody& body1,
 	btRigidBody& body2,
 	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo
-
-		)
+	const btContactSolverInfo& solverInfo)
 {
 
 	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
 	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
-    
-	
-//	printf("distance=%f\n",distance);
-
-	const btVector3& normal = contactPoint.m_normalWorldOnB;
+   	const btVector3& normal = contactPoint.m_normalWorldOnB;
 
 	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
 	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
@@ -117,34 +102,18 @@ float resolveSingleCollision(
 	btScalar rel_vel;
 	rel_vel = normal.dot(vel);
 	
-
 	btScalar Kfps = 1.f / solverInfo.m_timeStep ;
 
 	float damping = solverInfo.m_damping ;
 	float Kerp = solverInfo.m_erp;
-	
-	if (useGlobalSettingContacts)
-	{
-		damping = contactDamping;
-		Kerp = contactTau;
-	} 
-
 	float Kcor = Kerp *Kfps;
 
-	//printf("dist=%f\n",distance);
-
-		btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
+	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
 	assert(cpd);
-
-	btScalar distance = cpd->m_penetration;//contactPoint.getDistance();
-	
-
-	//distance = 0.f;
+	btScalar distance = cpd->m_penetration;
 	btScalar positionalError = Kcor *-distance;
-	//jacDiagABInv;
 	btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
 
-	
 	btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
 
 	btScalar	velocityImpulse = velocityError * cpd->m_jacDiagABInv;
@@ -158,9 +127,20 @@ float resolveSingleCollision(
 
 	normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
 
+#ifdef USE_INTERNAL_APPLY_IMPULSE
+	if (body1.getInvMass())
+	{
+		body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
+	}
+	if (body2.getInvMass())
+	{
+		body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
+	}
+#else USE_INTERNAL_APPLY_IMPULSE
 	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
 	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
-	
+#endif //USE_INTERNAL_APPLY_IMPULSE
+
 	return normalImpulse;
 }
 
@@ -169,9 +149,86 @@ float resolveSingleFriction(
 	btRigidBody& body1,
 	btRigidBody& body2,
 	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo
+	const btContactSolverInfo& solverInfo)
+{
+
+	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
+	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
+
+	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	
+	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
+	assert(cpd);
+
+	float combinedFriction = cpd->m_friction;
+	
+	btScalar limit = cpd->m_appliedImpulse * combinedFriction;
+	//if (contactPoint.m_appliedImpulse>0.f)
+	//friction
+	{
+		//apply friction in the 2 tangential directions
+		
+		// 1st tangent
+		btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+		btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+		btVector3 vel = vel1 - vel2;
+	
+		btScalar j1,j2;
+
+		{
+				
+			btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
+
+			// calculate j that moves us to zero relative velocity
+			j1 = -vrel * cpd->m_jacDiagABInvTangent0;
+			float total = cpd->m_accumulatedTangentImpulse0 + j1;
+			GEN_set_min(total, limit);
+			GEN_set_max(total, -limit);
+			j1 = total - cpd->m_accumulatedTangentImpulse0;
+			cpd->m_accumulatedTangentImpulse0 = total;
+		}
+		{
+			// 2nd tangent
+
+			btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
+			
+			// calculate j that moves us to zero relative velocity
+			j2 = -vrel * cpd->m_jacDiagABInvTangent1;
+			float total = cpd->m_accumulatedTangentImpulse1 + j2;
+			GEN_set_min(total, limit);
+			GEN_set_max(total, -limit);
+			j2 = total - cpd->m_accumulatedTangentImpulse1;
+			cpd->m_accumulatedTangentImpulse1 = total;
+		}
+
+#ifdef USE_INTERNAL_APPLY_IMPULSE
+	if (body1.getInvMass())
+	{
+		body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
+		body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
+	}
+	if (body2.getInvMass())
+	{
+		body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
+		body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);	
+	}
+#else USE_INTERNAL_APPLY_IMPULSE
+	body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
+	body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
+#endif //USE_INTERNAL_APPLY_IMPULSE
+
+
+	} 
+	return cpd->m_appliedImpulse;
+}
 
-		)
+
+float resolveSingleFrictionOriginal(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo)
 {
 
 	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
@@ -232,3 +289,110 @@ float resolveSingleFriction(
 	} 
 	return cpd->m_appliedImpulse;
 }
+
+
+//velocity + friction
+//response  between two dynamic objects with friction
+float resolveSingleCollisionCombined(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo)
+{
+
+	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
+	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
+   	const btVector3& normal = contactPoint.m_normalWorldOnB;
+
+	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	
+	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+	btVector3 vel = vel1 - vel2;
+	btScalar rel_vel;
+	rel_vel = normal.dot(vel);
+	
+	btScalar Kfps = 1.f / solverInfo.m_timeStep ;
+
+	float damping = solverInfo.m_damping ;
+	float Kerp = solverInfo.m_erp;
+	float Kcor = Kerp *Kfps;
+
+	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
+	assert(cpd);
+	btScalar distance = cpd->m_penetration;
+	btScalar positionalError = Kcor *-distance;
+	btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
+
+	btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
+
+	btScalar	velocityImpulse = velocityError * cpd->m_jacDiagABInv;
+
+	btScalar normalImpulse = penetrationImpulse+velocityImpulse;
+	
+	// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+	float oldNormalImpulse = cpd->m_appliedImpulse;
+	float sum = oldNormalImpulse + normalImpulse;
+	cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum;
+
+	normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
+
+
+#ifdef USE_INTERNAL_APPLY_IMPULSE
+	if (body1.getInvMass())
+	{
+		body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
+	}
+	if (body2.getInvMass())
+	{
+		body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
+	}
+#else USE_INTERNAL_APPLY_IMPULSE
+	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
+	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
+#endif //USE_INTERNAL_APPLY_IMPULSE
+
+	{
+		//friction
+		btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+		btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+		btVector3 vel = vel1 - vel2;
+	  
+		rel_vel = normal.dot(vel);
+
+
+		btVector3 lat_vel = vel - normal * rel_vel;
+		btScalar lat_rel_vel = lat_vel.length();
+
+		float combinedFriction = cpd->m_friction;
+
+		if (cpd->m_appliedImpulse > 0)
+		if (lat_rel_vel > SIMD_EPSILON)
+		{
+			lat_vel /= lat_rel_vel;
+			btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
+			btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
+			btScalar friction_impulse = lat_rel_vel /
+				(body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
+			btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
+
+			GEN_set_min(friction_impulse, normal_impulse);
+			GEN_set_max(friction_impulse, -normal_impulse);
+			body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
+			body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
+		}
+	}
+
+
+
+	return normalImpulse;
+}
+float resolveSingleFrictionEmpty(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo)
+{
+	return 0.f;
+};
\ No newline at end of file
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h
index 42ded30ae04..d88ba0d8ed4 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h
@@ -72,6 +72,15 @@ struct btConstraintPersistentData
 			float	m_penetration;
 			btVector3	m_frictionWorldTangential0;
 			btVector3	m_frictionWorldTangential1;
+
+			btVector3	m_frictionAngularComponent0A;
+			btVector3	m_frictionAngularComponent0B;
+			btVector3	m_frictionAngularComponent1A;
+			btVector3	m_frictionAngularComponent1B;
+
+			//some data doesn't need to be persistent over frames: todo: clean/reuse this
+			btVector3	m_angularComponentA;
+			btVector3	m_angularComponentB;
 		
 			ContactSolverFunc	m_contactSolverFunc;
 			ContactSolverFunc	m_frictionSolverFunc;
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index e747177a516..ac06f718c9e 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -74,8 +74,6 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
 		for (j=0;j<numManifolds;j++)
 		{
 			int k=j;
-			//interleaving the preparation with solving impacts the behaviour a lot, todo: find out why
-
 			prepareConstraints(manifoldPtr[k],info,debugDrawer);
 			solve(manifoldPtr[k],info,0,debugDrawer);
 		}
@@ -232,6 +230,7 @@ void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
 					cpd->m_penetration = 0.f;
 				} 				
 				
+				
 
 				float relaxation = info.m_damping;
 				cpd->m_appliedImpulse *= relaxation;
@@ -266,6 +265,36 @@ void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
 	#endif //NO_FRICTION_WARMSTART
 					cp.m_normalWorldOnB*cpd->m_appliedImpulse;
 
+
+
+				///
+				{
+				btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+				cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
+				btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
+				cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
+				}
+				{
+					btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
+					cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
+				}
+				{
+					btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
+					cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
+				}
+				{
+					btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
+					cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
+				}
+				{
+					btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
+					cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
+				}
+				
+				///
+
+
+
 				//apply previous frames impulse on both bodies
 				body0->applyImpulse(totalImpulse, rel_pos1);
 				body1->applyImpulse(-totalImpulse, rel_pos2);
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index feb1d2823f1..1bfc9b33348 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -127,15 +127,15 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 			switch(colObj->GetActivationState())
 			{
 			case  ACTIVE_TAG:
-				color = btVector3(255.f,255.f,255.f);
+				color = btVector3(255.f,255.f,255.f); break;
 			case ISLAND_SLEEPING:
-				color =  btVector3(0.f,255.f,0.f);
+				color =  btVector3(0.f,255.f,0.f);break;
 			case WANTS_DEACTIVATION:
-				color = btVector3(0.f,255.f,255.f);
+				color = btVector3(0.f,255.f,255.f);break;
 			case DISABLE_DEACTIVATION:
-				color = btVector3(255.f,0.f,0.f);
+				color = btVector3(255.f,0.f,0.f);break;
 			case DISABLE_SIMULATION:
-				color = btVector3(255.f,255.f,0.f);
+				color = btVector3(255.f,255.f,0.f);break;
 			default:
 				{
 					color = btVector3(255.f,0.f,0.f);
@@ -150,7 +150,8 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 			if (body->GetActivationState() != ISLAND_SLEEPING)
 			{
 				btTransform interpolatedTransform;
-				btTransformUtil::integrateTransform(body->m_interpolationWorldTransform,body->getLinearVelocity(),body->getAngularVelocity(),m_localTime,interpolatedTransform);
+				btTransformUtil::integrateTransform(body->m_interpolationWorldTransform,
+					body->m_interpolationLinearVelocity,body->m_interpolationAngularVelocity,m_localTime,interpolatedTransform);
 				body->getMotionState()->setWorldTransform(interpolatedTransform);
 			}
 		}
@@ -390,7 +391,7 @@ void	btDiscreteDynamicsWorld::solveNoncontactConstraints(btContactSolverInfo& so
 	BEGIN_PROFILE("solveNoncontactConstraints");
 
 	int i;
-	int numConstraints = m_constraints.size();
+	int numConstraints = int(m_constraints.size());
 
 	///constraint preparation: building jacobians
 	for (i=0;i< numConstraints ; i++ )
@@ -424,7 +425,7 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
 
 	{
 		int i;
-		int numConstraints = m_constraints.size();
+		int numConstraints = int(m_constraints.size());
 		for (i=0;i< numConstraints ; i++ )
 		{
 			btTypedConstraint* constraint = m_constraints[i];
@@ -502,7 +503,30 @@ void	btDiscreteDynamicsWorld::updateAabbs()
 				btPoint3 minAabb,maxAabb;
 				colObj->m_collisionShape->getAabb(colObj->m_worldTransform, minAabb,maxAabb);
 				btSimpleBroadphase* bp = (btSimpleBroadphase*)m_broadphasePairCache;
-				bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb);
+
+				//moving objects should be moderately sized, probably something wrong if not
+				if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < 1e12f))
+				{
+					bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb);
+				} else
+				{
+					//something went wrong, investigate
+					//this assert is unwanted in 3D modelers (danger of loosing work)
+					assert(0);
+					body->SetActivationState(DISABLE_SIMULATION);
+					
+					static bool reportMe = true;
+					if (reportMe)
+					{
+						reportMe = false;
+						printf("Overflow in AABB, object removed from simulation \n");
+						printf("If you can reproduce this, please email bugs@continuousphysics.com\n");
+						printf("Please include above information, your Platform, version of OS.\n");
+						printf("Thanks.\n");
+					}
+
+
+				}
 				if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
 				{
 					DrawAabb(m_debugDrawer,minAabb,maxAabb,colorvec);
@@ -670,10 +694,10 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 				float radius = coneShape->getRadius();//+coneShape->getMargin();
 				float height = coneShape->getHeight();//+coneShape->getMargin();
 				btVector3 start = worldTransform.getOrigin();
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,0,0.5*height),start+worldTransform.getBasis() * btVector3(radius,0,-0.5*height),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,0,0.5*height),start+worldTransform.getBasis() * btVector3(-radius,0,-0.5*height),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,0,0.5*height),start+worldTransform.getBasis() * btVector3(0,radius,-0.5*height),color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,0,0.5*height),start+worldTransform.getBasis() * btVector3(0,-radius,-0.5*height),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(radius,0.f,-0.5f*height),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(-radius,0.f,-0.5f*height),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(0.f,radius,-0.5f*height),color);
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(0.f,-radius,-0.5f*height),color);
 				break;
 
 			}
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
index 8a872264c22..fa916f65505 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
@@ -18,6 +18,7 @@ subject to the following restrictions:
 
 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
 class btTypedConstraint;
+class btRaycastVehicle;
 
 ///btDynamicsWorld is the baseclass for several dynamics implementation, basic, discrete, parallel, and continuous
 class btDynamicsWorld : public btCollisionWorld
@@ -47,6 +48,11 @@ class btDynamicsWorld : public btCollisionWorld
 
 		virtual void	removeConstraint(btTypedConstraint* constraint) {};
 
+		virtual void	addVehicle(btRaycastVehicle* vehicle) {};
+
+		virtual void	removeVehicle(btRaycastVehicle* vehicle) {};
+
+
 		virtual void	setDebugDrawer(btIDebugDraw*	debugDrawer) = 0;
 
 		virtual btIDebugDraw*	getDebugDrawer() = 0;
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
index 9e7c4978c31..a434f5e41dd 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
@@ -24,8 +24,8 @@ float gLinearAirDamping = 1.f;
 float	gDeactivationTime = 2.f;
 bool	gDisableDeactivation = false;
 
-float gLinearSleepingTreshold = 0.8f;
-float gAngularSleepingTreshold = 1.0f;
+float gLinearSleepingThreshold = 0.8f;
+float gAngularSleepingThreshold = 1.0f;
 static int uniqueId = 0;
 
 btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution)
@@ -45,7 +45,9 @@ btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShap
 	motionState->getWorldTransform(m_worldTransform);
 
 	m_interpolationWorldTransform = m_worldTransform;
-
+	m_interpolationLinearVelocity.setValue(0,0,0);
+	m_interpolationAngularVelocity.setValue(0,0,0);
+	
 	//moved to btCollisionObject
 	m_friction = friction;
 	m_restitution = restitution;
@@ -79,6 +81,8 @@ btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisi
 	
 	m_worldTransform = worldTransform;
 	m_interpolationWorldTransform = m_worldTransform;
+	m_interpolationLinearVelocity.setValue(0,0,0);
+	m_interpolationAngularVelocity.setValue(0,0,0);
 
 	//moved to btCollisionObject
 	m_friction = friction;
@@ -96,12 +100,32 @@ btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisi
 
 }
 
+#define EXPERIMENTAL_JITTER_REMOVAL 1
+#ifdef EXPERIMENTAL_JITTER_REMOVAL
+//Bullet 2.20b has experimental code to reduce jitter just before objects fall asleep/deactivate
+//doesn't work very well yet (value 0 only reduces performance a bit, no difference in functionality)
+float gClippedAngvelThresholdSqr = 0.f;
+float	gClippedLinearThresholdSqr = 0.f;
+#endif //EXPERIMENTAL_JITTER_REMOVAL
 
+void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
+{
 
+#ifdef EXPERIMENTAL_JITTER_REMOVAL
+	//clip to avoid jitter
+	if (m_angularVelocity.length2() < gClippedAngvelThresholdSqr)
+	{
+		m_angularVelocity.setValue(0,0,0);
+		printf("clipped!\n");
+	}
+	
+	if (m_linearVelocity.length2() < gClippedLinearThresholdSqr)
+	{
+		m_linearVelocity.setValue(0,0,0);
+		printf("clipped!\n");
+	}
+#endif //EXPERIMENTAL_JITTER_REMOVAL
 
-
-void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) const
-{
 	btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
 }
 
@@ -114,7 +138,10 @@ void			btRigidBody::saveKinematicState(btScalar timeStep)
 		if (getMotionState())
 			getMotionState()->getWorldTransform(m_worldTransform);
 		btVector3 linVel,angVel;
+		
 		btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
+		m_interpolationLinearVelocity = m_linearVelocity;
+		m_interpolationAngularVelocity = m_angularVelocity;
 		m_interpolationWorldTransform = m_worldTransform;
 		//printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
 	}
@@ -257,6 +284,8 @@ btQuaternion btRigidBody::getOrientation() const
 void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
 {
 	m_interpolationWorldTransform = m_worldTransform;
+	m_interpolationLinearVelocity = getLinearVelocity();
+	m_interpolationAngularVelocity = getAngularVelocity();
 	m_worldTransform = xform;
 	updateInertiaTensor();
 }
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
index c6d3bd50a35..fca3658cb31 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
@@ -34,8 +34,8 @@ extern bool gUseEpa;
 
 extern float gDeactivationTime;
 extern bool gDisableDeactivation;
-extern float gLinearSleepingTreshold;
-extern float gAngularSleepingTreshold;
+extern float gLinearSleepingThreshold;
+extern float gAngularSleepingThreshold;
 
 
 /// btRigidBody class for btRigidBody Dynamics
@@ -79,7 +79,7 @@ public:
 	}
 	
 	/// continuous collision detection needs prediction
-	void			predictIntegratedTransform(btScalar step, btTransform& predictedTransform) const;
+	void			predictIntegratedTransform(btScalar step, btTransform& predictedTransform) ;
 	
 	void			saveKinematicState(btScalar step);
 	
@@ -158,6 +158,16 @@ public:
 			applyTorqueImpulse(rel_pos.cross(impulse));
 		}
 	}
+
+	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+	inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,float impulseMagnitude)
+	{
+		if (m_inverseMass != 0.f)
+		{
+			m_linearVelocity += linearComponent*impulseMagnitude;
+			m_angularVelocity += angularComponent*impulseMagnitude;
+		}
+	}
 	
 	void clearForces() 
 	{
@@ -240,8 +250,8 @@ public:
 		if ( (GetActivationState() == ISLAND_SLEEPING) || (GetActivationState() == DISABLE_DEACTIVATION))
 			return;
 
-		if ((getLinearVelocity().length2() < gLinearSleepingTreshold*gLinearSleepingTreshold) &&
-			(getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold))
+		if ((getLinearVelocity().length2() < gLinearSleepingThreshold*gLinearSleepingThreshold) &&
+			(getAngularVelocity().length2() < gAngularSleepingThreshold*gAngularSleepingThreshold))
 		{
 			m_deactivationTime += timeStep;
 		} else
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
index 03841f99a70..7a2043e2b38 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
@@ -9,11 +9,12 @@
  * It is provided "as is" without express or implied warranty.
 */
 
+#include "LinearMath/btVector3.h"
 #include "btRaycastVehicle.h"
 #include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btJacobianEntry.h"
 #include "LinearMath/btQuaternion.h"
-#include "LinearMath/btVector3.h"
+#include "BulletDynamics/Dynamics/btDynamicsWorld.h"
 #include "btVehicleRaycaster.h"
 #include "btWheelInfo.h"
 
@@ -141,8 +142,12 @@ void	btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel )
 {
 	wheel.m_raycastInfo.m_isInContact = false;
 
-	const btTransform& chassisTrans = getRigidBody()->getCenterOfMassTransform();
-
+	btTransform chassisTrans;
+	if (getRigidBody()->getMotionState())
+		getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
+	else
+		chassisTrans = getRigidBody()->getCenterOfMassTransform();
+	
 	wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS );
 	wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() *  wheel.m_wheelDirectionCS ;
 	wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS;
@@ -166,6 +171,8 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 	
 	btVehicleRaycaster::btVehicleRaycasterResult	rayResults;
 
+	assert(m_vehicleRaycaster);
+
 	void* object = m_vehicleRaycaster->castRay(source,target,rayResults);
 
 	wheel.m_raycastInfo.m_groundObject = 0;
@@ -593,3 +600,28 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 		delete[]forwardImpulse;
 		delete[] sideImpulse;
 }
+
+
+void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result)
+{
+//	RayResultCallback& resultCallback;
+
+	btCollisionWorld::ClosestRayResultCallback rayCallback(from,to);
+
+	m_dynamicsWorld->rayTest(from, to, rayCallback);
+
+	if (rayCallback.HasHit())
+	{
+		
+		btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
+		if (body)
+		{
+			result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
+			result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
+			result.m_hitNormalInWorld.normalize();
+			result.m_distFraction = rayCallback.m_closestHitFraction;
+			return body;
+		}
+	}
+	return 0;
+}
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
index 8468bc52016..a7534c67555 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
@@ -13,11 +13,11 @@
 
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
-
+#include "btVehicleRaycaster.h"
+class btDynamicsWorld;
 
 #include "btWheelInfo.h"
 
-struct	btVehicleRaycaster;
 class btVehicleTuning;
 
 ///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle.
@@ -93,7 +93,7 @@ public:
 	btWheelInfo&	addWheel( const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS,btScalar suspensionRestLength,btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel);
 
 	inline int		getNumWheels() const {
-		return m_wheelInfo.size();
+		return int (m_wheelInfo.size());
 	}
 	
 	std::vector<btWheelInfo>	m_wheelInfo;
@@ -163,5 +163,19 @@ public:
 
 };
 
+class btDefaultVehicleRaycaster : public btVehicleRaycaster
+{
+	btDynamicsWorld*	m_dynamicsWorld;
+public:
+	btDefaultVehicleRaycaster(btDynamicsWorld* world)
+		:m_dynamicsWorld(world)
+	{
+	}
+
+	virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result);
+
+};
+
+
 #endif //RAYCASTVEHICLE_H