added some new Bullet files, and upgraded to latest Bullet 2.x

Please make sure to have extern/bullet/src/LinearMath/btAlignedAllocator.cpp in your build, if you add the files by name, instead of wildcard *.cpp

12 files changed, 231 insertions, 209 deletions

diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
index e073797f989..ce90f798c04 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
@@ -18,7 +18,7 @@ subject to the following restrictions:
 
 class btPersistentManifold;
 class btRigidBody;
-
+class btTypedConstraint;
 struct btContactSolverInfo;
 struct btBroadphaseProxy;
 class btIDebugDraw;
@@ -31,7 +31,7 @@ public:
 
 	virtual ~btConstraintSolver() {}
 	
-	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0;
+	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0;
 
 };
 
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
index b5783f824d4..b2132a8d4f3 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
@@ -289,7 +289,7 @@ void	btGeneric6DofConstraint::solveConstraint(btScalar	timeStep)
 				btScalar loLimit =  m_upperLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : -1e30f;
 				btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : 1e30f;
 
-				float projAngle  = -2.*xyz[i];
+				float projAngle  = -2.f*xyz[i];
 				
 				if (projAngle < loLimit)
 				{
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
index 7b2a9ba6244..f72278e2cbf 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
@@ -19,7 +19,8 @@ subject to the following restrictions:
 #include "LinearMath/btTransformUtil.h"
 
 
-btHingeConstraint::btHingeConstraint()
+btHingeConstraint::btHingeConstraint():
+m_enableAngularMotor(false)
 {
 }
 
@@ -28,7 +29,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
 :btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
 m_axisInA(axisInA),
 m_axisInB(-axisInB),
-m_angularOnly(false)
+m_angularOnly(false),
+m_enableAngularMotor(false)
 {
 
 }
@@ -39,7 +41,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,
 m_axisInA(axisInA),
 //fixed axis in worldspace
 m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA),
-m_angularOnly(false)
+m_angularOnly(false),
+m_enableAngularMotor(false)
 {
 	
 }
@@ -73,11 +76,16 @@ void	btHingeConstraint::buildJacobian()
 	//these two jointAxis require equal angular velocities for both bodies
 
 	//this is unused for now, it's a todo
-	btVector3 axisWorldA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
-	btVector3 jointAxis0;
-	btVector3 jointAxis1;
-	btPlaneSpace1(axisWorldA,jointAxis0,jointAxis1);
+	btVector3 jointAxis0local;
+	btVector3 jointAxis1local;
 	
+	btPlaneSpace1(m_axisInA,jointAxis0local,jointAxis1local);
+
+	getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
+	btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
+	btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
+	btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
+		
 	new (&m_jacAng[0])	btJacobianEntry(jointAxis0,
 		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
 		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -90,107 +98,18 @@ void	btHingeConstraint::buildJacobian()
 		m_rbA.getInvInertiaDiagLocal(),
 		m_rbB.getInvInertiaDiagLocal());
 
+	new (&m_jacAng[2])	btJacobianEntry(hingeAxisWorld,
+		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbA.getInvInertiaDiagLocal(),
+		m_rbB.getInvInertiaDiagLocal());
+
+
 
 }
 
 void	btHingeConstraint::solveConstraint(btScalar	timeStep)
 {
-//#define NEW_IMPLEMENTATION
-
-#ifdef NEW_IMPLEMENTATION
-	btScalar tau = 0.3f;
-	btScalar damping = 1.f;
-
-	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
-	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
-
-	// Dirk: Don't we need to update this after each applied impulse
-	btVector3 angvelA; // = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-    btVector3 angvelB; // = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
-
-
-	if (!m_angularOnly)
-	{
-		btVector3 normal(0,0,0);
-
-		for (int i=0;i<3;i++)
-		{		
-			normal[i] = 1;
-			btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
-
-			btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
-			btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-			
-			btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-			btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-			btVector3 vel = vel1 - vel2;
-
-			// Dirk: Get new angular velocity since it changed after applying an impulse
-			angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-			angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
-	
-			//velocity error (first order error)
-			btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
-																	m_rbB.getLinearVelocity(),angvelB);
-		
-			//positional error (zeroth order error)
-			btScalar depth = -(pivotAInW - pivotBInW).dot(normal); 
-			
-			btScalar impulse = tau*depth/timeStep * jacDiagABInv -  damping * rel_vel * jacDiagABInv;
-
-			btVector3 impulse_vector = normal * impulse;
-			m_rbA.applyImpulse( impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
-			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
-			
-			normal[i] = 0;
-		}
-	}
-
-	///solve angular part
-
-	// get axes in world space
-	btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
-	btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
-
-	// constraint axes in world space
-	btVector3 jointAxis0;
-	btVector3 jointAxis1;
-	btPlaneSpace1(axisA,jointAxis0,jointAxis1);
-
-
-	// Dirk: Get new angular velocity since it changed after applying an impulse
-	angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-    angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
-	
-	btScalar jacDiagABInv0 = 1.f / m_jacAng[0].getDiagonal();
-	btScalar rel_vel0 = m_jacAng[0].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
-																	m_rbB.getLinearVelocity(),angvelB);
-	float tau1 = tau;//0.f;
-
-	btScalar impulse0 = (tau1 * axisB.dot(jointAxis1) / timeStep - damping * rel_vel0) * jacDiagABInv0;
-	btVector3 angular_impulse0 = jointAxis0 * impulse0;
-
-	m_rbA.applyTorqueImpulse( angular_impulse0);
-	m_rbB.applyTorqueImpulse(-angular_impulse0);
-
-
-
-	// Dirk: Get new angular velocity since it changed after applying an impulse	
-	angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-    angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
-
-	btScalar jacDiagABInv1 = 1.f / m_jacAng[1].getDiagonal();
-	btScalar rel_vel1 = m_jacAng[1].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
-																	m_rbB.getLinearVelocity(),angvelB);;
-
-	btScalar impulse1 = -(tau1 * axisB.dot(jointAxis0) / timeStep + damping * rel_vel1) * jacDiagABInv1;
-	btVector3 angular_impulse1 = jointAxis1 * impulse1;
-
-	m_rbA.applyTorqueImpulse( angular_impulse1);
-	m_rbB.applyTorqueImpulse(-angular_impulse1);
-
-#else
-
 
 	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
 	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
@@ -237,37 +156,68 @@ void	btHingeConstraint::solveConstraint(btScalar	timeStep)
 
 		const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
 		const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
-		btVector3 angA = angVelA - axisA * axisA.dot(angVelA);
-		btVector3 angB = angVelB - axisB * axisB.dot(angVelB);
-		btVector3 velrel = angA-angB;
-
-		//solve angular velocity correction
-		float relaxation = 1.f;
-		float len = velrel.length();
-		if (len > 0.00001f)
+
+		btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
+		btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
+
+		btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
+		btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
+		btVector3 velrelOrthog = angAorthog-angBorthog;
 		{
-			btVector3 normal = velrel.normalized();
-			float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
-				getRigidBodyB().computeAngularImpulseDenominator(normal);
-			// scale for mass and relaxation
-			velrel *= (1.f/denom) * 0.9;
+			//solve orthogonal angular velocity correction
+			float relaxation = 1.f;
+			float len = velrelOrthog.length();
+			if (len > 0.00001f)
+			{
+				btVector3 normal = velrelOrthog.normalized();
+				float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
+					getRigidBodyB().computeAngularImpulseDenominator(normal);
+				// scale for mass and relaxation
+				//todo:  expose this 0.9 factor to developer
+				velrelOrthog *= (1.f/denom) * 0.9f;
+			}
+
+			//solve angular positional correction
+			btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep);
+			float len2 = angularError.length();
+			if (len2>0.00001f)
+			{
+				btVector3 normal2 = angularError.normalized();
+				float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
+						getRigidBodyB().computeAngularImpulseDenominator(normal2);
+				angularError *= (1.f/denom2) * relaxation;
+			}
+
+			m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
+			m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
 		}
 
-		//solve angular positional correction
-		btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep);
-		float len2 = angularError.length();
-		if (len2>0.00001f)
+		//apply motor
+		if (m_enableAngularMotor)
 		{
-			btVector3 normal2 = angularError.normalized();
-			float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
-					getRigidBodyB().computeAngularImpulseDenominator(normal2);
-			angularError *= (1.f/denom2) * relaxation;
-		}
+			//todo: add limits too
+			btVector3 angularLimit(0,0,0);
+
+			btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
+			btScalar projRelVel = velrel.dot(axisA);
+
+			btScalar desiredMotorVel = m_motorTargetVelocity;
+			btScalar motor_relvel = desiredMotorVel - projRelVel;
 
-		m_rbA.applyTorqueImpulse(-velrel+angularError);
-		m_rbB.applyTorqueImpulse(velrel-angularError);
+			float denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) +
+					getRigidBodyB().computeAngularImpulseDenominator(axisA);
+
+			btScalar unclippedMotorImpulse = (1.f/denom3) * motor_relvel;;
+			//todo: should clip against accumulated impulse
+			btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
+			clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
+			btVector3 motorImp = clippedMotorImpulse * axisA;
+
+			m_rbA.applyTorqueImpulse(motorImp+angularLimit);
+			m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
+			
+		}
 	}
-#endif
 
 }
 
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
index 3bade2b091d..553ec135c8a 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
@@ -29,14 +29,18 @@ class btRigidBody;
 class btHingeConstraint : public btTypedConstraint
 {
 	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
-	btJacobianEntry	m_jacAng[2]; //2 orthogonal angular constraints
+	btJacobianEntry	m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
 
 	btVector3	m_pivotInA;
 	btVector3	m_pivotInB;
 	btVector3	m_axisInA;
 	btVector3	m_axisInB;
 
-	bool	m_angularOnly;
+	bool		m_angularOnly;
+
+	float		m_motorTargetVelocity;
+	float		m_maxMotorImpulse;
+	bool		m_enableAngularMotor;
 	
 public:
 
@@ -66,7 +70,12 @@ public:
 		m_angularOnly = angularOnly;
 	}
 
-
+	void	enableAngularMotor(bool enableMotor,float targetVelocity,float maxMotorImpulse)
+	{
+		m_enableAngularMotor  = enableMotor;
+		m_motorTargetVelocity = targetVelocity;
+		m_maxMotorImpulse = maxMotorImpulse;
+	}
 
 };
 
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index ce85fc398be..94eece73e7c 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -23,6 +23,8 @@ subject to the following restrictions:
 #include "LinearMath/btIDebugDraw.h"
 #include "btJacobianEntry.h"
 #include "LinearMath/btMinMax.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+
 
 #ifdef USE_PROFILE
 #include "LinearMath/btQuickprof.h"
@@ -123,7 +125,7 @@ btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
 }
 
 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
-float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
 {
 	
 	btContactSolverInfo info = infoGlobal;
@@ -151,6 +153,15 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma
 			}
 		}
 	}
+
+	{
+		int j;
+		for (j=0;j<numConstraints;j++)
+		{
+			btTypedConstraint* constraint = constraints[j];
+			constraint->buildJacobian();
+		}
+	}
 	
 	//should traverse the contacts random order...
 	int iteration;
@@ -171,6 +182,12 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma
 				}
 			}
 
+			for (j=0;j<numConstraints;j++)
+			{
+				btTypedConstraint* constraint = constraints[j];
+				constraint->solveConstraint(info.m_timeStep);
+			}
+
 			for (j=0;j<totalPoints;j++)
 			{
 				btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
@@ -197,7 +214,7 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma
 
 
 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
-float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
 {
 	
 	btContactSolverInfo info = infoGlobal;
@@ -222,6 +239,14 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
 			}
 		}
 	}
+	{
+		int j;
+		for (j=0;j<numConstraints;j++)
+		{
+			btTypedConstraint* constraint = constraints[j];
+			constraint->buildJacobian();
+		}
+	}
 	
 	//should traverse the contacts random order...
 	int iteration;
@@ -230,6 +255,12 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
 	{
 		int j;
 
+		for (j=0;j<numConstraints;j++)
+		{
+			btTypedConstraint* constraint = constraints[j];
+			constraint->solveConstraint(info.m_timeStep);
+		}
+
 		for (j=0;j<numManifolds;j++)
 		{
 			btPersistentManifold* manifold = manifoldPtr[j];
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
index ec56af14dac..0989a86e2cd 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
@@ -68,7 +68,7 @@ public:
 
 	virtual ~btSequentialImpulseConstraintSolver() {}
 	
-	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
+	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
 
 	void	setSolverMode(int mode)
 	{
@@ -88,7 +88,7 @@ public:
 
 	btSequentialImpulseConstraintSolver3();
 
-	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
+	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
 
 
 };
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index f59598f2a53..fc37cf2e3c3 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -261,7 +261,7 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep)
 	dispatchInfo.m_debugDraw = getDebugDrawer();
 
 	///perform collision detection
-	performDiscreteCollisionDetection(dispatchInfo);
+	performDiscreteCollisionDetection();
 
 	calculateSimulationIslands();
 
@@ -270,12 +270,9 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep)
 	
 
 
-	///solve non-contact constraints
-	solveNoncontactConstraints(getSolverInfo());
+	///solve contact and other joint constraints
+	solveConstraints(getSolverInfo());
 	
-	///solve contact constraints
-	solveContactConstraints(getSolverInfo());
-
 	///CallbackTriggers();
 
 	///integrate transforms
@@ -401,80 +398,117 @@ void	btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
 	}
 }
 
+inline	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
+{
+	int islandId;
+	
+	const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
+	const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
+	islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
+	return islandId;
+
+}
 
-void	btDiscreteDynamicsWorld::solveContactConstraints(btContactSolverInfo& solverInfo)
+static bool btSortConstraintOnIslandPredicate(const btTypedConstraint* lhs, const btTypedConstraint* rhs)
+{
+	int rIslandId0,lIslandId0;
+	rIslandId0 = btGetConstraintIslandId(rhs);
+	lIslandId0 = btGetConstraintIslandId(lhs);
+	return lIslandId0 < rIslandId0;
+}
+
+void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
 	
-	BEGIN_PROFILE("solveContactConstraints");
+	BEGIN_PROFILE("solveConstraints");
 
 	struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
 	{
 
-		btContactSolverInfo& m_solverInfo;
-		btConstraintSolver*	m_solver;
-		btIDebugDraw*	m_debugDrawer;
+		btContactSolverInfo&	m_solverInfo;
+		btConstraintSolver*		m_solver;
+		btTypedConstraint**		m_sortedConstraints;
+		int						m_numConstraints;
+		btIDebugDraw*			m_debugDrawer;
+
+
 
 		InplaceSolverIslandCallback(
 			btContactSolverInfo& solverInfo,
 			btConstraintSolver*	solver,
+			btTypedConstraint** sortedConstraints,
+			int	numConstraints,
 			btIDebugDraw*	debugDrawer)
 			:m_solverInfo(solverInfo),
 			m_solver(solver),
+			m_sortedConstraints(sortedConstraints),
+			m_numConstraints(numConstraints),
 			m_debugDrawer(debugDrawer)
 		{
 
 		}
 
-		virtual	void	ProcessIsland(btPersistentManifold**	manifolds,int numManifolds)
+		virtual	void	ProcessIsland(btPersistentManifold**	manifolds,int numManifolds, int islandId)
 		{
-			m_solver->solveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer);
+			//also add all non-contact constraints/joints for this island
+			btTypedConstraint** startConstraint = 0;
+			int numCurConstraints = 0;
+			int i;
+			
+			//find the first constraint for this island
+			for (i=0;i<m_numConstraints;i++)
+			{
+				if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+				{
+					startConstraint = &m_sortedConstraints[i];
+					break;
+				}
+			}
+			//count the number of constraints in this island
+			for (;i<m_numConstraints;i++)
+			{
+				if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+				{
+					numCurConstraints++;
+				}
+			}
+
+			m_solver->solveGroup( manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer);
 		}
 
 	};
 
 	
-	InplaceSolverIslandCallback	solverCallback(	solverInfo,	m_constraintSolver,	m_debugDrawer);
 
 	
-	/// solve all the contact points and contact friction
-	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback);
-
-	END_PROFILE("solveContactConstraints");
 
-}
-
-
-void	btDiscreteDynamicsWorld::solveNoncontactConstraints(btContactSolverInfo& solverInfo)
-{
-	BEGIN_PROFILE("solveNoncontactConstraints");
-
-	int i;
-	int numConstraints = int(m_constraints.size());
-
-	///constraint preparation: building jacobians
-	for (i=0;i< numConstraints ; i++ )
+	//sorted version of all btTypedConstraint, based on islandId
+	std::vector<btTypedConstraint*>	sortedConstraints;
+	sortedConstraints.resize( m_constraints.size());
+	int i; 
+	for (i=0;i<getNumConstraints();i++)
 	{
-		btTypedConstraint* constraint = m_constraints[i];
-		constraint->buildJacobian();
+		sortedConstraints[i] = m_constraints[i];
 	}
+	
+	std::sort(sortedConstraints.begin(),sortedConstraints.end(),btSortConstraintOnIslandPredicate);
+	
+	btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
+	
+	InplaceSolverIslandCallback	solverCallback(	solverInfo,	m_constraintSolver, constraintsPtr,sortedConstraints.size(),	m_debugDrawer);
 
-	//solve the regular non-contact constraints (point 2 point, hinge, generic d6)
-	for (int g=0;g<solverInfo.m_numIterations;g++)
-	{
-		//
-		// constraint solving
-		//
-		for (i=0;i< numConstraints ; i++ )
-		{
-			btTypedConstraint* constraint = m_constraints[i];
-			constraint->solveConstraint( solverInfo.m_timeStep );
-		}
-	}
+	
+	
+	/// solve all the constraints for this island
+	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback);
 
-	END_PROFILE("solveNoncontactConstraints");
+	END_PROFILE("solveConstraints");
 
 }
 
+
+
+
 void	btDiscreteDynamicsWorld::calculateSimulationIslands()
 {
 	BEGIN_PROFILE("calculateSimulationIslands");
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
index 72619091c6f..8575f8506f1 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
@@ -66,10 +66,8 @@ protected:
 		
 	void	calculateSimulationIslands();
 
-	void	solveNoncontactConstraints(btContactSolverInfo& solverInfo);
-
-	void	solveContactConstraints(btContactSolverInfo& solverInfo);
-
+	void	solveConstraints(btContactSolverInfo& solverInfo);
+	
 	void	updateActivationState(float timeStep);
 
 	void	updateVehicles(float timeStep);
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
index 453b8ec4289..705c023d3a0 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
@@ -110,16 +110,17 @@ btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisi
 #endif //OBSOLETE_MOTIONSTATE_LESS
 
 
-#define EXPERIMENTAL_JITTER_REMOVAL 1
+//#define EXPERIMENTAL_JITTER_REMOVAL 1
 #ifdef EXPERIMENTAL_JITTER_REMOVAL
 //Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate
 //doesn't work very well yet (value 0 disabled this damping)
 //note there this influences deactivation thresholds!
 float gClippedAngvelThresholdSqr = 0.01f;
 float	gClippedLinearThresholdSqr = 0.01f;
-float	gJitterVelocityDampingFactor = 1.f;
 #endif //EXPERIMENTAL_JITTER_REMOVAL
 
+float	gJitterVelocityDampingFactor = 1.f;
+
 void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
 {
 
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
index ce5c76c58e2..fe0124c041a 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
@@ -46,13 +46,13 @@ int		btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, floa
 	///apply gravity, predict motion
 	predictUnconstraintMotion(timeStep);
 
-	btDispatcherInfo	dispatchInfo;
+	btDispatcherInfo&	dispatchInfo = getDispatchInfo();
 	dispatchInfo.m_timeStep = timeStep;
 	dispatchInfo.m_stepCount = 0;
 	dispatchInfo.m_debugDraw = getDebugDrawer();
 
 	///perform collision detection
-	performDiscreteCollisionDetection(dispatchInfo );
+	performDiscreteCollisionDetection();
 
 	///solve contact constraints
 	int numManifolds = m_dispatcher1->getNumManifolds();
@@ -63,7 +63,7 @@ int		btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, floa
 		btContactSolverInfo infoGlobal;
 		infoGlobal.m_timeStep = timeStep;
 		
-		m_constraintSolver->solveGroup(manifoldPtr, numManifolds,infoGlobal,m_debugDrawer);
+		m_constraintSolver->solveGroup(manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer);
 	}
 
 	///integrate transforms
@@ -101,7 +101,10 @@ void	btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
 {
 	body->setGravity(m_gravity);
 
-	addCollisionObject(body);
+	if (body->getCollisionShape())
+	{
+		addCollisionObject(body);
+	}
 }
 
 void	btSimpleDynamicsWorld::updateAabbs()
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
index 248ecfbce4e..c85fead5334 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
@@ -128,11 +128,10 @@ void	btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedT
 void btRaycastVehicle::resetSuspension()
 {
 
-	std::vector<btWheelInfo>::iterator wheelIt;
-	for (wheelIt = m_wheelInfo.begin();
-	!(wheelIt == m_wheelInfo.end());wheelIt++)
+	int i;
+	for (i=0;i<m_wheelInfo.size();	i++)
 	{
-			btWheelInfo& wheel = *wheelIt;
+			btWheelInfo& wheel = m_wheelInfo[i];
 			wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
 			wheel.m_suspensionRelativeVelocity = 0.0f;
 			
@@ -285,23 +284,21 @@ void btRaycastVehicle::updateVehicle( btScalar step )
 	//
 	// simulate suspension
 	//
-	std::vector<btWheelInfo>::iterator wheelIt;
+	
 	int i=0;
-	for (wheelIt = m_wheelInfo.begin();
-	!(wheelIt == m_wheelInfo.end());wheelIt++,i++)
+	for (i=0;i<m_wheelInfo.size();i++)
 	{
 		btScalar depth; 
-		depth = rayCast( *wheelIt );
+		depth = rayCast( m_wheelInfo[i]);
 	}
 
 	updateSuspension(step);
 
 	
-	for (wheelIt = m_wheelInfo.begin();
-	!(wheelIt == m_wheelInfo.end());wheelIt++)
+	for (i=0;i<m_wheelInfo.size();i++)
 	{
 		//apply suspension force
-		btWheelInfo& wheel = *wheelIt;
+		btWheelInfo& wheel = m_wheelInfo[i];
 		
 		float suspensionForce = wheel.m_wheelsSuspensionForce;
 		
@@ -322,10 +319,9 @@ void btRaycastVehicle::updateVehicle( btScalar step )
 	updateFriction( step);
 
 	
-	for (wheelIt = m_wheelInfo.begin();
-	!(wheelIt == m_wheelInfo.end());wheelIt++)
+	for (i=0;i<m_wheelInfo.size();i++)
 	{
-		btWheelInfo& wheel = *wheelIt;
+		btWheelInfo& wheel = m_wheelInfo[i];
 		btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
 		btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos );
 
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
index b928248cc2d..fa961e468d7 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
@@ -15,7 +15,7 @@
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
 #include "btVehicleRaycaster.h"
 class btDynamicsWorld;
-
+#include "LinearMath/btAlignedObjectArray.h"
 #include "btWheelInfo.h"
 
 class btVehicleTuning;
@@ -95,7 +95,7 @@ public:
 		return int (m_wheelInfo.size());
 	}
 	
-	std::vector<btWheelInfo>	m_wheelInfo;
+	btAlignedObjectArray<btWheelInfo>	m_wheelInfo;
 
 
 	const btWheelInfo&	getWheelInfo(int index) const;