Sync Bullet to upstream

This syncs Bullet to the latest upstream git version as of writing this.
(commit 47b0259b9700455022b5cf79b651cc1dc71dd59e).

102 files changed, 20971 insertions, 13743 deletions

diff --git a/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h b/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h
index dffb06dfe94..2ccf317b927 100644
--- a/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h
+++ b/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h
@@ -26,22 +26,21 @@ class btCollisionWorld;
 class btCharacterControllerInterface : public btActionInterface
 {
 public:
-	btCharacterControllerInterface () {};
-	virtual ~btCharacterControllerInterface () {};
-	
-	virtual void	setWalkDirection(const btVector3& walkDirection) = 0;
-	virtual void	setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0;
-	virtual void	reset ( btCollisionWorld* collisionWorld ) = 0;
-	virtual void	warp (const btVector3& origin) = 0;
-
-	virtual void	preStep ( btCollisionWorld* collisionWorld) = 0;
-	virtual void	playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0;
-	virtual bool	canJump () const = 0;
-	virtual void	jump () = 0;
-
-	virtual bool	onGround () const = 0;
-	virtual void	setUpInterpolate (bool value) = 0;
-};
+	btCharacterControllerInterface(){};
+	virtual ~btCharacterControllerInterface(){};
+
+	virtual void setWalkDirection(const btVector3& walkDirection) = 0;
+	virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval) = 0;
+	virtual void reset(btCollisionWorld* collisionWorld) = 0;
+	virtual void warp(const btVector3& origin) = 0;
 
-#endif //BT_CHARACTER_CONTROLLER_INTERFACE_H
+	virtual void preStep(btCollisionWorld* collisionWorld) = 0;
+	virtual void playerStep(btCollisionWorld* collisionWorld, btScalar dt) = 0;
+	virtual bool canJump() const = 0;
+	virtual void jump(const btVector3& dir = btVector3(0, 0, 0)) = 0;
+
+	virtual bool onGround() const = 0;
+	virtual void setUpInterpolate(bool value) = 0;
+};
 
+#endif  //BT_CHARACTER_CONTROLLER_INTERFACE_H
diff --git a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp
index 31faf1df5e3..2bbccb291c7 100644
--- a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp
+++ b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include <stdio.h>
 #include "LinearMath/btIDebugDraw.h"
 #include "BulletCollision/CollisionDispatch/btGhostObject.h"
@@ -24,20 +23,19 @@ subject to the following restrictions:
 #include "LinearMath/btDefaultMotionState.h"
 #include "btKinematicCharacterController.h"
 
-
 // static helper method
 static btVector3
 getNormalizedVector(const btVector3& v)
 {
 	btVector3 n(0, 0, 0);
 
-	if (v.length() > SIMD_EPSILON) {
+	if (v.length() > SIMD_EPSILON)
+	{
 		n = v.normalized();
 	}
 	return n;
 }
 
-
 ///@todo Interact with dynamic objects,
 ///Ride kinematicly animated platforms properly
 ///More realistic (or maybe just a config option) falling
@@ -47,18 +45,19 @@ getNormalizedVector(const btVector3& v)
 class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
 {
 public:
-	btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
+	btKinematicClosestNotMeRayResultCallback(btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
 	{
 		m_me = me;
 	}
 
-	virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
+	virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace)
 	{
 		if (rayResult.m_collisionObject == m_me)
 			return 1.0;
 
-		return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace);
+		return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);
 	}
+
 protected:
 	btCollisionObject* m_me;
 };
@@ -66,15 +65,12 @@ protected:
 class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
 {
 public:
-	btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me, const btVector3& up, btScalar minSlopeDot)
-	: btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
-	, m_me(me)
-	, m_up(up)
-	, m_minSlopeDot(minSlopeDot)
+	btKinematicClosestNotMeConvexResultCallback(btCollisionObject* me, const btVector3& up, btScalar minSlopeDot)
+		: btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)), m_me(me), m_up(up), m_minSlopeDot(minSlopeDot)
 	{
 	}
 
-	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
+	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace)
 	{
 		if (convexResult.m_hitCollisionObject == m_me)
 			return btScalar(1.0);
@@ -86,19 +82,22 @@ public:
 		if (normalInWorldSpace)
 		{
 			hitNormalWorld = convexResult.m_hitNormalLocal;
-		} else
+		}
+		else
 		{
 			///need to transform normal into worldspace
-			hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
+			hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis() * convexResult.m_hitNormalLocal;
 		}
 
 		btScalar dotUp = m_up.dot(hitNormalWorld);
-		if (dotUp < m_minSlopeDot) {
+		if (dotUp < m_minSlopeDot)
+		{
 			return btScalar(1.0);
 		}
 
-		return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
+		return ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace);
 	}
+
 protected:
 	btCollisionObject* m_me;
 	const btVector3 m_up;
@@ -110,7 +109,7 @@ protected:
  *
  * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
  */
-btVector3 btKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal)
+btVector3 btKinematicCharacterController::computeReflectionDirection(const btVector3& direction, const btVector3& normal)
 {
 	return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
 }
@@ -118,7 +117,7 @@ btVector3 btKinematicCharacterController::computeReflectionDirection (const btVe
 /*
  * Returns the portion of 'direction' that is parallel to 'normal'
  */
-btVector3 btKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal)
+btVector3 btKinematicCharacterController::parallelComponent(const btVector3& direction, const btVector3& normal)
 {
 	btScalar magnitude = direction.dot(normal);
 	return normal * magnitude;
@@ -127,38 +126,46 @@ btVector3 btKinematicCharacterController::parallelComponent (const btVector3& di
 /*
  * Returns the portion of 'direction' that is perpindicular to 'normal'
  */
-btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal)
+btVector3 btKinematicCharacterController::perpindicularComponent(const btVector3& direction, const btVector3& normal)
 {
 	return direction - parallelComponent(direction, normal);
 }
 
-btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
+btKinematicCharacterController::btKinematicCharacterController(btPairCachingGhostObject* ghostObject, btConvexShape* convexShape, btScalar stepHeight, const btVector3& up)
 {
-	m_upAxis = upAxis;
+	m_ghostObject = ghostObject;
+	m_up.setValue(0.0f, 0.0f, 1.0f);
+	m_jumpAxis.setValue(0.0f, 0.0f, 1.0f);
 	m_addedMargin = 0.02;
-	m_walkDirection.setValue(0,0,0);
+	m_walkDirection.setValue(0.0, 0.0, 0.0);
+	m_AngVel.setValue(0.0, 0.0, 0.0);
 	m_useGhostObjectSweepTest = true;
-	m_ghostObject = ghostObject;
-	m_stepHeight = stepHeight;
 	m_turnAngle = btScalar(0.0);
-	m_convexShape=convexShape;	
-	m_useWalkDirection = true;	// use walk direction by default, legacy behavior
+	m_convexShape = convexShape;
+	m_useWalkDirection = true;  // use walk direction by default, legacy behavior
 	m_velocityTimeInterval = 0.0;
 	m_verticalVelocity = 0.0;
 	m_verticalOffset = 0.0;
-	m_gravity = 9.8 * 3 ; // 3G acceleration.
-	m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s.
-	m_jumpSpeed = 10.0; // ?
+	m_gravity = 9.8 * 3.0;  // 3G acceleration.
+	m_fallSpeed = 55.0;     // Terminal velocity of a sky diver in m/s.
+	m_jumpSpeed = 10.0;     // ?
+	m_SetjumpSpeed = m_jumpSpeed;
 	m_wasOnGround = false;
 	m_wasJumping = false;
 	m_interpolateUp = true;
-	setMaxSlope(btRadians(45.0));
-	m_currentStepOffset = 0;
+	m_currentStepOffset = 0.0;
+	m_maxPenetrationDepth = 0.2;
 	full_drop = false;
 	bounce_fix = false;
+	m_linearDamping = btScalar(0.0);
+	m_angularDamping = btScalar(0.0);
+
+	setUp(up);
+	setStepHeight(stepHeight);
+	setMaxSlope(btRadians(45.0));
 }
 
-btKinematicCharacterController::~btKinematicCharacterController ()
+btKinematicCharacterController::~btKinematicCharacterController()
 {
 }
 
@@ -167,7 +174,7 @@ btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
 	return m_ghostObject;
 }
 
-bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld)
+bool btKinematicCharacterController::recoverFromPenetration(btCollisionWorld* collisionWorld)
 {
 	// Here we must refresh the overlapping paircache as the penetrating movement itself or the
 	// previous recovery iteration might have used setWorldTransform and pushed us into an object
@@ -178,19 +185,19 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
 	// paircache and the ghostobject's internal paircache at the same time.    /BW
 
 	btVector3 minAabb, maxAabb;
-	m_convexShape->getAabb(m_ghostObject->getWorldTransform(), minAabb,maxAabb);
-	collisionWorld->getBroadphase()->setAabb(m_ghostObject->getBroadphaseHandle(), 
-						 minAabb, 
-						 maxAabb, 
-						 collisionWorld->getDispatcher());
-						 
+	m_convexShape->getAabb(m_ghostObject->getWorldTransform(), minAabb, maxAabb);
+	collisionWorld->getBroadphase()->setAabb(m_ghostObject->getBroadphaseHandle(),
+											 minAabb,
+											 maxAabb,
+											 collisionWorld->getDispatcher());
+
 	bool penetration = false;
 
 	collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher());
 
 	m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
-	
-	btScalar maxPen = btScalar(0.0);
+
+	//	btScalar maxPen = btScalar(0.0);
 	for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
 	{
 		m_manifoldArray.resize(0);
@@ -198,195 +205,240 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld*
 		btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
 
 		btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
-                btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
+		btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
 
 		if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse()))
 			continue;
-		
+
+		if (!needsCollision(obj0, obj1))
+			continue;
+
 		if (collisionPair->m_algorithm)
 			collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
 
-		
-		for (int j=0;j<m_manifoldArray.size();j++)
+		for (int j = 0; j < m_manifoldArray.size(); j++)
 		{
 			btPersistentManifold* manifold = m_manifoldArray[j];
 			btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0);
-			for (int p=0;p<manifold->getNumContacts();p++)
+			for (int p = 0; p < manifold->getNumContacts(); p++)
 			{
-				const btManifoldPoint&pt = manifold->getContactPoint(p);
+				const btManifoldPoint& pt = manifold->getContactPoint(p);
 
 				btScalar dist = pt.getDistance();
 
-				if (dist < 0.0)
+				if (dist < -m_maxPenetrationDepth)
 				{
-					if (dist < maxPen)
-					{
-						maxPen = dist;
-						m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
+					// TODO: cause problems on slopes, not sure if it is needed
+					//if (dist < maxPen)
+					//{
+					//	maxPen = dist;
+					//	m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
 
-					}
+					//}
 					m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2);
 					penetration = true;
-				} else {
+				}
+				else
+				{
 					//printf("touching %f\n", dist);
 				}
 			}
-			
+
 			//manifold->clearManifold();
 		}
 	}
 	btTransform newTrans = m_ghostObject->getWorldTransform();
 	newTrans.setOrigin(m_currentPosition);
 	m_ghostObject->setWorldTransform(newTrans);
-//	printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
+	//	printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
 	return penetration;
 }
 
-void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
+void btKinematicCharacterController::stepUp(btCollisionWorld* world)
 {
+	btScalar stepHeight = 0.0f;
+	if (m_verticalVelocity < 0.0)
+		stepHeight = m_stepHeight;
+
 	// phase 1: up
 	btTransform start, end;
-	m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f));
 
-	start.setIdentity ();
-	end.setIdentity ();
+	start.setIdentity();
+	end.setIdentity();
 
 	/* FIXME: Handle penetration properly */
-	start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin));
-	end.setOrigin (m_targetPosition);
+	start.setOrigin(m_currentPosition);
+
+	m_targetPosition = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0.f ? m_verticalOffset : 0.f));
+	m_currentPosition = m_targetPosition;
 
-	btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071));
+	end.setOrigin(m_targetPosition);
+
+	start.setRotation(m_currentOrientation);
+	end.setRotation(m_targetOrientation);
+
+	btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine);
 	callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
 	callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
-	
+
 	if (m_useGhostObjectSweepTest)
 	{
-		m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
+		m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
 	}
 	else
 	{
-		world->convexSweepTest (m_convexShape, start, end, callback);
+		world->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
 	}
-	
-	if (callback.hasHit())
+
+	if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
 	{
 		// Only modify the position if the hit was a slope and not a wall or ceiling.
-		if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0)
+		if (callback.m_hitNormalWorld.dot(m_up) > 0.0)
 		{
 			// we moved up only a fraction of the step height
-			m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
+			m_currentStepOffset = stepHeight * callback.m_closestHitFraction;
 			if (m_interpolateUp == true)
-				m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+				m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
 			else
 				m_currentPosition = m_targetPosition;
 		}
-		m_verticalVelocity = 0.0;
-		m_verticalOffset = 0.0;
-	} else {
-		m_currentStepOffset = m_stepHeight;
+
+		btTransform& xform = m_ghostObject->getWorldTransform();
+		xform.setOrigin(m_currentPosition);
+		m_ghostObject->setWorldTransform(xform);
+
+		// fix penetration if we hit a ceiling for example
+		int numPenetrationLoops = 0;
+		m_touchingContact = false;
+		while (recoverFromPenetration(world))
+		{
+			numPenetrationLoops++;
+			m_touchingContact = true;
+			if (numPenetrationLoops > 4)
+			{
+				//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+				break;
+			}
+		}
+		m_targetPosition = m_ghostObject->getWorldTransform().getOrigin();
+		m_currentPosition = m_targetPosition;
+
+		if (m_verticalOffset > 0)
+		{
+			m_verticalOffset = 0.0;
+			m_verticalVelocity = 0.0;
+			m_currentStepOffset = m_stepHeight;
+		}
+	}
+	else
+	{
+		m_currentStepOffset = stepHeight;
 		m_currentPosition = m_targetPosition;
 	}
 }
 
-void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
+bool btKinematicCharacterController::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1)
+{
+	bool collides = (body0->getBroadphaseHandle()->m_collisionFilterGroup & body1->getBroadphaseHandle()->m_collisionFilterMask) != 0;
+	collides = collides && (body1->getBroadphaseHandle()->m_collisionFilterGroup & body0->getBroadphaseHandle()->m_collisionFilterMask);
+	return collides;
+}
+
+void btKinematicCharacterController::updateTargetPositionBasedOnCollision(const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
 {
 	btVector3 movementDirection = m_targetPosition - m_currentPosition;
 	btScalar movementLength = movementDirection.length();
-	if (movementLength>SIMD_EPSILON)
+	if (movementLength > SIMD_EPSILON)
 	{
 		movementDirection.normalize();
 
-		btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal);
+		btVector3 reflectDir = computeReflectionDirection(movementDirection, hitNormal);
 		reflectDir.normalize();
 
 		btVector3 parallelDir, perpindicularDir;
 
-		parallelDir = parallelComponent (reflectDir, hitNormal);
-		perpindicularDir = perpindicularComponent (reflectDir, hitNormal);
+		parallelDir = parallelComponent(reflectDir, hitNormal);
+		perpindicularDir = perpindicularComponent(reflectDir, hitNormal);
 
 		m_targetPosition = m_currentPosition;
-		if (0)//tangentMag != 0.0)
+		if (0)  //tangentMag != 0.0)
 		{
-			btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength);
-//			printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
-			m_targetPosition +=  parComponent;
+			btVector3 parComponent = parallelDir * btScalar(tangentMag * movementLength);
+			//			printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
+			m_targetPosition += parComponent;
 		}
 
 		if (normalMag != 0.0)
 		{
-			btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength);
-//			printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
+			btVector3 perpComponent = perpindicularDir * btScalar(normalMag * movementLength);
+			//			printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
 			m_targetPosition += perpComponent;
 		}
-	} else
+	}
+	else
 	{
-//		printf("movementLength don't normalize a zero vector\n");
+		//		printf("movementLength don't normalize a zero vector\n");
 	}
 }
 
-void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove)
+void btKinematicCharacterController::stepForwardAndStrafe(btCollisionWorld* collisionWorld, const btVector3& walkMove)
 {
 	// printf("m_normalizedDirection=%f,%f,%f\n",
 	// 	m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
 	// phase 2: forward and strafe
 	btTransform start, end;
+
 	m_targetPosition = m_currentPosition + walkMove;
 
-	start.setIdentity ();
-	end.setIdentity ();
-	
-	btScalar fraction = 1.0;
-	btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
-//	printf("distance2=%f\n",distance2);
+	start.setIdentity();
+	end.setIdentity();
 
-	if (m_touchingContact)
-	{
-		if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0))
-		{
-			//interferes with step movement
-			//updateTargetPositionBasedOnCollision (m_touchingNormal);
-		}
-	}
+	btScalar fraction = 1.0;
+	btScalar distance2 = (m_currentPosition - m_targetPosition).length2();
+	//	printf("distance2=%f\n",distance2);
 
 	int maxIter = 10;
 
 	while (fraction > btScalar(0.01) && maxIter-- > 0)
 	{
-		start.setOrigin (m_currentPosition);
-		end.setOrigin (m_targetPosition);
+		start.setOrigin(m_currentPosition);
+		end.setOrigin(m_targetPosition);
 		btVector3 sweepDirNegative(m_currentPosition - m_targetPosition);
 
-		btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0));
+		start.setRotation(m_currentOrientation);
+		end.setRotation(m_targetOrientation);
+
+		btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, sweepDirNegative, btScalar(0.0));
 		callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
 		callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
-
 		btScalar margin = m_convexShape->getMargin();
 		m_convexShape->setMargin(margin + m_addedMargin);
 
-
-		if (m_useGhostObjectSweepTest)
+		if (!(start == end))
 		{
-			m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-		} else
-		{
-			collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			if (m_useGhostObjectSweepTest)
+			{
+				m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			}
+			else
+			{
+				collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			}
 		}
-		
 		m_convexShape->setMargin(margin);
 
-		
 		fraction -= callback.m_closestHitFraction;
 
-		if (callback.hasHit())
-		{	
+		if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))
+		{
 			// we moved only a fraction
 			//btScalar hitDistance;
 			//hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
 
-//			m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+			//			m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
 
-			updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
+			updateTargetPositionBasedOnCollision(callback.m_hitNormalWorld);
 			btVector3 currentDir = m_targetPosition - m_currentPosition;
 			distance2 = currentDir.length2();
 			if (distance2 > SIMD_EPSILON)
@@ -397,115 +449,120 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co
 				{
 					break;
 				}
-			} else
+			}
+			else
 			{
-//				printf("currentDir: don't normalize a zero vector\n");
+				//				printf("currentDir: don't normalize a zero vector\n");
 				break;
 			}
-
-		} else {
-			// we moved whole way
+		}
+		else
+		{
 			m_currentPosition = m_targetPosition;
 		}
-
-	//	if (callback.m_closestHitFraction == 0.f)
-	//		break;
-
 	}
 }
 
-void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
+void btKinematicCharacterController::stepDown(btCollisionWorld* collisionWorld, btScalar dt)
 {
 	btTransform start, end, end_double;
 	bool runonce = false;
 
 	// phase 3: down
 	/*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
-	btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
+	btVector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep);
 	btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
-	btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; 
+	btVector3 gravity_drop = m_up * downVelocity; 
 	m_targetPosition -= (step_drop + gravity_drop);*/
 
 	btVector3 orig_position = m_targetPosition;
-	
-	btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
 
-	if(downVelocity > 0.0 && downVelocity > m_fallSpeed
-		&& (m_wasOnGround || !m_wasJumping))
+	btScalar downVelocity = (m_verticalVelocity < 0.f ? -m_verticalVelocity : 0.f) * dt;
+
+	if (m_verticalVelocity > 0.0)
+		return;
+
+	if (downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
 		downVelocity = m_fallSpeed;
 
-	btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+	btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity);
 	m_targetPosition -= step_drop;
 
-	btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
-        callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
-        callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
+	btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine);
+	callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
+	callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
-        btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine);
-        callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
-        callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
+	btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine);
+	callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
+	callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
 
 	while (1)
 	{
-		start.setIdentity ();
-		end.setIdentity ();
+		start.setIdentity();
+		end.setIdentity();
 
-		end_double.setIdentity ();
+		end_double.setIdentity();
 
-		start.setOrigin (m_currentPosition);
-		end.setOrigin (m_targetPosition);
+		start.setOrigin(m_currentPosition);
+		end.setOrigin(m_targetPosition);
+
+		start.setRotation(m_currentOrientation);
+		end.setRotation(m_targetOrientation);
 
 		//set double test for 2x the step drop, to check for a large drop vs small drop
-		end_double.setOrigin (m_targetPosition - step_drop);
+		end_double.setOrigin(m_targetPosition - step_drop);
 
 		if (m_useGhostObjectSweepTest)
 		{
-			m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			m_ghostObject->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 
-			if (!callback.hasHit())
+			if (!callback.hasHit() && m_ghostObject->hasContactResponse())
 			{
 				//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
-				m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+				m_ghostObject->convexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 			}
-		} else
+		}
+		else
 		{
-			collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			collisionWorld->convexSweepTest(m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
 
-			if (!callback.hasHit())
-					{
-							//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
-							collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
-					}
+			if (!callback.hasHit() && m_ghostObject->hasContactResponse())
+			{
+				//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
+				collisionWorld->convexSweepTest(m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
+			}
 		}
-	
-		btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
-		bool has_hit = false;
+
+		btScalar downVelocity2 = (m_verticalVelocity < 0.f ? -m_verticalVelocity : 0.f) * dt;
+		bool has_hit;
 		if (bounce_fix == true)
-			has_hit = callback.hasHit() || callback2.hasHit();
+			has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject);
 		else
-			has_hit = callback2.hasHit();
+			has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback2.m_hitCollisionObject);
+
+		btScalar stepHeight = 0.0f;
+		if (m_verticalVelocity < 0.0)
+			stepHeight = m_stepHeight;
 
-		if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false
-					&& (m_wasOnGround || !m_wasJumping))
+		if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false && (m_wasOnGround || !m_wasJumping))
 		{
 			//redo the velocity calculation when falling a small amount, for fast stairs motion
 			//for larger falls, use the smoother/slower interpolated movement by not touching the target position
 
 			m_targetPosition = orig_position;
-					downVelocity = m_stepHeight;
+			downVelocity = stepHeight;
 
-				btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+			step_drop = m_up * (m_currentStepOffset + downVelocity);
 			m_targetPosition -= step_drop;
 			runonce = true;
-			continue; //re-run previous tests
+			continue;  //re-run previous tests
 		}
 		break;
 	}
 
-	if (callback.hasHit() || runonce == true)
+	if ((m_ghostObject->hasContactResponse() && (callback.hasHit() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))) || runonce == true)
 	{
 		// we dropped a fraction of the height -> hit floor
-
 		btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2;
 
 		//printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());
@@ -513,32 +570,34 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 		if (bounce_fix == true)
 		{
 			if (full_drop == true)
-                                m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
-                        else
-                                //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
-                                m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction);
+				m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+			else
+				//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
+				m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, fraction);
 		}
 		else
-			m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
+			m_currentPosition.setInterpolate3(m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
 
 		full_drop = false;
 
 		m_verticalVelocity = 0.0;
 		m_verticalOffset = 0.0;
 		m_wasJumping = false;
-	} else {
+	}
+	else
+	{
 		// we dropped the full height
-		
+
 		full_drop = true;
 
 		if (bounce_fix == true)
 		{
-			downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt;
+			downVelocity = (m_verticalVelocity < 0.f ? -m_verticalVelocity : 0.f) * dt;
 			if (downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping))
 			{
-				m_targetPosition += step_drop; //undo previous target change
+				m_targetPosition += step_drop;  //undo previous target change
 				downVelocity = m_fallSpeed;
-				step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity);
+				step_drop = m_up * (m_currentStepOffset + downVelocity);
 				m_targetPosition -= step_drop;
 			}
 		}
@@ -548,30 +607,22 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld
 	}
 }
 
-
-
-void btKinematicCharacterController::setWalkDirection
-(
-const btVector3& walkDirection
-)
+void btKinematicCharacterController::setWalkDirection(
+	const btVector3& walkDirection)
 {
 	m_useWalkDirection = true;
 	m_walkDirection = walkDirection;
 	m_normalizedDirection = getNormalizedVector(m_walkDirection);
 }
 
-
-
-void btKinematicCharacterController::setVelocityForTimeInterval
-(
-const btVector3& velocity,
-btScalar timeInterval
-)
+void btKinematicCharacterController::setVelocityForTimeInterval(
+	const btVector3& velocity,
+	btScalar timeInterval)
 {
-//	printf("setVelocity!\n");
-//	printf("  interval: %f\n", timeInterval);
-//	printf("  velocity: (%f, %f, %f)\n",
-//		 velocity.x(), velocity.y(), velocity.z());
+	//	printf("setVelocity!\n");
+	//	printf("  interval: %f\n", timeInterval);
+	//	printf("  velocity: (%f, %f, %f)\n",
+	//		 velocity.x(), velocity.y(), velocity.z());
 
 	m_useWalkDirection = false;
 	m_walkDirection = velocity;
@@ -579,93 +630,175 @@ btScalar timeInterval
 	m_velocityTimeInterval += timeInterval;
 }
 
-void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld )
+void btKinematicCharacterController::setAngularVelocity(const btVector3& velocity)
 {
-        m_verticalVelocity = 0.0;
-        m_verticalOffset = 0.0;
-        m_wasOnGround = false;
-        m_wasJumping = false;
-        m_walkDirection.setValue(0,0,0);
-        m_velocityTimeInterval = 0.0;
-
-        //clear pair cache
-        btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache();
-        while (cache->getOverlappingPairArray().size() > 0)
-        {
-                cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
-        }
+	m_AngVel = velocity;
 }
 
-void btKinematicCharacterController::warp (const btVector3& origin)
+const btVector3& btKinematicCharacterController::getAngularVelocity() const
 {
-	btTransform xform;
-	xform.setIdentity();
-	xform.setOrigin (origin);
-	m_ghostObject->setWorldTransform (xform);
+	return m_AngVel;
 }
 
-
-void btKinematicCharacterController::preStep (  btCollisionWorld* collisionWorld)
+void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity)
 {
-	
-	int numPenetrationLoops = 0;
-	m_touchingContact = false;
-	while (recoverFromPenetration (collisionWorld))
+	m_walkDirection = velocity;
+
+	// HACK: if we are moving in the direction of the up, treat it as a jump :(
+	if (m_walkDirection.length2() > 0)
 	{
-		numPenetrationLoops++;
-		m_touchingContact = true;
-		if (numPenetrationLoops > 4)
+		btVector3 w = velocity.normalized();
+		btScalar c = w.dot(m_up);
+		if (c != 0)
 		{
-			//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
-			break;
+			//there is a component in walkdirection for vertical velocity
+			btVector3 upComponent = m_up * (btSin(SIMD_HALF_PI - btAcos(c)) * m_walkDirection.length());
+			m_walkDirection -= upComponent;
+			m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length();
+
+			if (c > 0.0f)
+			{
+				m_wasJumping = true;
+				m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
+			}
 		}
 	}
+	else
+		m_verticalVelocity = 0.0f;
+}
 
+btVector3 btKinematicCharacterController::getLinearVelocity() const
+{
+	return m_walkDirection + (m_verticalVelocity * m_up);
+}
+
+void btKinematicCharacterController::reset(btCollisionWorld* collisionWorld)
+{
+	m_verticalVelocity = 0.0;
+	m_verticalOffset = 0.0;
+	m_wasOnGround = false;
+	m_wasJumping = false;
+	m_walkDirection.setValue(0, 0, 0);
+	m_velocityTimeInterval = 0.0;
+
+	//clear pair cache
+	btHashedOverlappingPairCache* cache = m_ghostObject->getOverlappingPairCache();
+	while (cache->getOverlappingPairArray().size() > 0)
+	{
+		cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher());
+	}
+}
+
+void btKinematicCharacterController::warp(const btVector3& origin)
+{
+	btTransform xform;
+	xform.setIdentity();
+	xform.setOrigin(origin);
+	m_ghostObject->setWorldTransform(xform);
+}
+
+void btKinematicCharacterController::preStep(btCollisionWorld* collisionWorld)
+{
 	m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
 	m_targetPosition = m_currentPosition;
-//	printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
 
-	
+	m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
+	m_targetOrientation = m_currentOrientation;
+	//	printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
 }
 
-#include <stdio.h>
-
-void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWorld, btScalar dt)
+void btKinematicCharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar dt)
 {
-//	printf("playerStep(): ");
-//	printf("  dt = %f", dt);
+	//	printf("playerStep(): ");
+	//	printf("  dt = %f", dt);
+
+	if (m_AngVel.length2() > 0.0f)
+	{
+		m_AngVel *= btPow(btScalar(1) - m_angularDamping, dt);
+	}
+
+	// integrate for angular velocity
+	if (m_AngVel.length2() > 0.0f)
+	{
+		btTransform xform;
+		xform = m_ghostObject->getWorldTransform();
+
+		btQuaternion rot(m_AngVel.normalized(), m_AngVel.length() * dt);
+
+		btQuaternion orn = rot * xform.getRotation();
+
+		xform.setRotation(orn);
+		m_ghostObject->setWorldTransform(xform);
+
+		m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
+		m_targetPosition = m_currentPosition;
+		m_currentOrientation = m_ghostObject->getWorldTransform().getRotation();
+		m_targetOrientation = m_currentOrientation;
+	}
 
 	// quick check...
-	if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) {
-//		printf("\n");
-		return;		// no motion
+	if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) 
+	{
+		//		printf("\n");
+		return;  // no motion
 	}
 
 	m_wasOnGround = onGround();
 
+	//btVector3 lvel = m_walkDirection;
+	//btScalar c = 0.0f;
+
+	if (m_walkDirection.length2() > 0)
+	{
+		// apply damping
+		m_walkDirection *= btPow(btScalar(1) - m_linearDamping, dt);
+	}
+
+	m_verticalVelocity *= btPow(btScalar(1) - m_linearDamping, dt);
+
 	// Update fall velocity.
 	m_verticalVelocity -= m_gravity * dt;
-	if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
+	if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
 	{
 		m_verticalVelocity = m_jumpSpeed;
 	}
-	if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
+	if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
 	{
 		m_verticalVelocity = -btFabs(m_fallSpeed);
 	}
 	m_verticalOffset = m_verticalVelocity * dt;
 
-
 	btTransform xform;
-	xform = m_ghostObject->getWorldTransform ();
-
-//	printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
-//	printf("walkSpeed=%f\n",walkSpeed);
-
-	stepUp (collisionWorld);
-	if (m_useWalkDirection) {
-		stepForwardAndStrafe (collisionWorld, m_walkDirection);
-	} else {
+	xform = m_ghostObject->getWorldTransform();
+
+	//	printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
+	//	printf("walkSpeed=%f\n",walkSpeed);
+
+	stepUp(collisionWorld);
+	//todo: Experimenting with behavior of controller when it hits a ceiling..
+	//bool hitUp = stepUp (collisionWorld);
+	//if (hitUp)
+	//{
+	//	m_verticalVelocity -= m_gravity * dt;
+	//	if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
+	//	{
+	//		m_verticalVelocity = m_jumpSpeed;
+	//	}
+	//	if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
+	//	{
+	//		m_verticalVelocity = -btFabs(m_fallSpeed);
+	//	}
+	//	m_verticalOffset = m_verticalVelocity * dt;
+
+	//	xform = m_ghostObject->getWorldTransform();
+	//}
+
+	if (m_useWalkDirection)
+	{
+		stepForwardAndStrafe(collisionWorld, m_walkDirection);
+	}
+	else
+	{
 		//printf("  time: %f", m_velocityTimeInterval);
 		// still have some time left for moving!
 		btScalar dtMoving =
@@ -680,42 +813,73 @@ void btKinematicCharacterController::playerStep (  btCollisionWorld* collisionWo
 		// okay, step
 		stepForwardAndStrafe(collisionWorld, move);
 	}
-	stepDown (collisionWorld, dt);
-
+	stepDown(collisionWorld, dt);
+
+	//todo: Experimenting with max jump height
+	//if (m_wasJumping)
+	//{
+	//	btScalar ds = m_currentPosition[m_upAxis] - m_jumpPosition[m_upAxis];
+	//	if (ds > m_maxJumpHeight)
+	//	{
+	//		// substract the overshoot
+	//		m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight;
+
+	//		// max height was reached, so potential energy is at max
+	//		// and kinematic energy is 0, thus velocity is 0.
+	//		if (m_verticalVelocity > 0.0)
+	//			m_verticalVelocity = 0.0;
+	//	}
+	//}
 	// printf("\n");
 
-	xform.setOrigin (m_currentPosition);
-	m_ghostObject->setWorldTransform (xform);
+	xform.setOrigin(m_currentPosition);
+	m_ghostObject->setWorldTransform(xform);
+
+	int numPenetrationLoops = 0;
+	m_touchingContact = false;
+	while (recoverFromPenetration(collisionWorld))
+	{
+		numPenetrationLoops++;
+		m_touchingContact = true;
+		if (numPenetrationLoops > 4)
+		{
+			//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
+			break;
+		}
+	}
 }
 
-void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
+void btKinematicCharacterController::setFallSpeed(btScalar fallSpeed)
 {
 	m_fallSpeed = fallSpeed;
 }
 
-void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
+void btKinematicCharacterController::setJumpSpeed(btScalar jumpSpeed)
 {
 	m_jumpSpeed = jumpSpeed;
+	m_SetjumpSpeed = m_jumpSpeed;
 }
 
-void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
+void btKinematicCharacterController::setMaxJumpHeight(btScalar maxJumpHeight)
 {
 	m_maxJumpHeight = maxJumpHeight;
 }
 
-bool btKinematicCharacterController::canJump () const
+bool btKinematicCharacterController::canJump() const
 {
 	return onGround();
 }
 
-void btKinematicCharacterController::jump ()
+void btKinematicCharacterController::jump(const btVector3& v)
 {
-	if (!canJump())
-		return;
-
+	m_jumpSpeed = v.length2() == 0 ? m_SetjumpSpeed : v.length();
 	m_verticalVelocity = m_jumpSpeed;
 	m_wasJumping = true;
 
+	m_jumpAxis = v.length2() == 0 ? m_up : v.normalized();
+
+	m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin();
+
 #if 0
 	currently no jumping.
 	btTransform xform;
@@ -727,14 +891,16 @@ void btKinematicCharacterController::jump ()
 #endif
 }
 
-void btKinematicCharacterController::setGravity(btScalar gravity)
+void btKinematicCharacterController::setGravity(const btVector3& gravity)
 {
-	m_gravity = gravity;
+	if (gravity.length2() > 0) setUpVector(-gravity);
+
+	m_gravity = gravity.length();
 }
 
-btScalar btKinematicCharacterController::getGravity() const
+btVector3 btKinematicCharacterController::getGravity() const
 {
-	return m_gravity;
+	return -m_gravity * m_up;
 }
 
 void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians)
@@ -748,16 +914,30 @@ btScalar btKinematicCharacterController::getMaxSlope() const
 	return m_maxSlopeRadians;
 }
 
-bool btKinematicCharacterController::onGround () const
+void btKinematicCharacterController::setMaxPenetrationDepth(btScalar d)
+{
+	m_maxPenetrationDepth = d;
+}
+
+btScalar btKinematicCharacterController::getMaxPenetrationDepth() const
+{
+	return m_maxPenetrationDepth;
+}
+
+bool btKinematicCharacterController::onGround() const
 {
-	return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0;
+	return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON);
 }
 
+void btKinematicCharacterController::setStepHeight(btScalar h)
+{
+	m_stepHeight = h;
+}
 
 btVector3* btKinematicCharacterController::getUpAxisDirections()
 {
-	static btVector3 sUpAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) };
-	
+	static btVector3 sUpAxisDirection[3] = {btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f)};
+
 	return sUpAxisDirection;
 }
 
@@ -769,3 +949,48 @@ void btKinematicCharacterController::setUpInterpolate(bool value)
 {
 	m_interpolateUp = value;
 }
+
+void btKinematicCharacterController::setUp(const btVector3& up)
+{
+	if (up.length2() > 0 && m_gravity > 0.0f)
+	{
+		setGravity(-m_gravity * up.normalized());
+		return;
+	}
+
+	setUpVector(up);
+}
+
+void btKinematicCharacterController::setUpVector(const btVector3& up)
+{
+	if (m_up == up)
+		return;
+
+	btVector3 u = m_up;
+
+	if (up.length2() > 0)
+		m_up = up.normalized();
+	else
+		m_up = btVector3(0.0, 0.0, 0.0);
+
+	if (!m_ghostObject) return;
+	btQuaternion rot = getRotation(m_up, u);
+
+	//set orientation with new up
+	btTransform xform;
+	xform = m_ghostObject->getWorldTransform();
+	btQuaternion orn = rot.inverse() * xform.getRotation();
+	xform.setRotation(orn);
+	m_ghostObject->setWorldTransform(xform);
+}
+
+btQuaternion btKinematicCharacterController::getRotation(btVector3& v0, btVector3& v1) const
+{
+	if (v0.length2() == 0.0f || v1.length2() == 0.0f)
+	{
+		btQuaternion q;
+		return q;
+	}
+
+	return shortestArcQuatNormalize2(v0, v1);
+}
diff --git a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h
index add6f30a687..ff34fc871a5 100644
--- a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h
+++ b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef BT_KINEMATIC_CHARACTER_CONTROLLER_H
 #define BT_KINEMATIC_CHARACTER_CONTROLLER_H
 
@@ -23,7 +22,6 @@ subject to the following restrictions:
 
 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 
-
 class btCollisionShape;
 class btConvexShape;
 class btRigidBody;
@@ -34,99 +32,111 @@ class btPairCachingGhostObject;
 ///btKinematicCharacterController is an object that supports a sliding motion in a world.
 ///It uses a ghost object and convex sweep test to test for upcoming collisions. This is combined with discrete collision detection to recover from penetrations.
 ///Interaction between btKinematicCharacterController and dynamic rigid bodies needs to be explicity implemented by the user.
-ATTRIBUTE_ALIGNED16(class) btKinematicCharacterController : public btCharacterControllerInterface
+ATTRIBUTE_ALIGNED16(class)
+btKinematicCharacterController : public btCharacterControllerInterface
 {
 protected:
-
 	btScalar m_halfHeight;
-	
+
 	btPairCachingGhostObject* m_ghostObject;
-	btConvexShape*	m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
-	
+	btConvexShape* m_convexShape;  //is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
+
+	btScalar m_maxPenetrationDepth;
 	btScalar m_verticalVelocity;
 	btScalar m_verticalOffset;
 	btScalar m_fallSpeed;
 	btScalar m_jumpSpeed;
+	btScalar m_SetjumpSpeed;
 	btScalar m_maxJumpHeight;
-	btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value)
-	btScalar m_maxSlopeCosine;  // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization)
+	btScalar m_maxSlopeRadians;  // Slope angle that is set (used for returning the exact value)
+	btScalar m_maxSlopeCosine;   // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization)
 	btScalar m_gravity;
 
 	btScalar m_turnAngle;
-	
+
 	btScalar m_stepHeight;
 
-	btScalar	m_addedMargin;//@todo: remove this and fix the code
+	btScalar m_addedMargin;  //@todo: remove this and fix the code
 
 	///this is the desired walk direction, set by the user
-	btVector3	m_walkDirection;
-	btVector3	m_normalizedDirection;
+	btVector3 m_walkDirection;
+	btVector3 m_normalizedDirection;
+	btVector3 m_AngVel;
+
+	btVector3 m_jumpPosition;
 
 	//some internal variables
 	btVector3 m_currentPosition;
-	btScalar  m_currentStepOffset;
+	btScalar m_currentStepOffset;
 	btVector3 m_targetPosition;
 
+	btQuaternion m_currentOrientation;
+	btQuaternion m_targetOrientation;
+
 	///keep track of the contact manifolds
-	btManifoldArray	m_manifoldArray;
+	btManifoldArray m_manifoldArray;
 
 	bool m_touchingContact;
 	btVector3 m_touchingNormal;
 
-	bool  m_wasOnGround;
-	bool  m_wasJumping;
-	bool	m_useGhostObjectSweepTest;
-	bool	m_useWalkDirection;
-	btScalar	m_velocityTimeInterval;
-	int m_upAxis;
+	btScalar m_linearDamping;
+	btScalar m_angularDamping;
+
+	bool m_wasOnGround;
+	bool m_wasJumping;
+	bool m_useGhostObjectSweepTest;
+	bool m_useWalkDirection;
+	btScalar m_velocityTimeInterval;
+	btVector3 m_up;
+	btVector3 m_jumpAxis;
 
 	static btVector3* getUpAxisDirections();
-	bool  m_interpolateUp;
-	bool  full_drop;
-	bool  bounce_fix;
-
-	btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
-	btVector3 parallelComponent (const btVector3& direction, const btVector3& normal);
-	btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal);
-
-	bool recoverFromPenetration ( btCollisionWorld* collisionWorld);
-	void stepUp (btCollisionWorld* collisionWorld);
-	void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
-	void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove);
-	void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
-public:
+	bool m_interpolateUp;
+	bool full_drop;
+	bool bounce_fix;
+
+	btVector3 computeReflectionDirection(const btVector3& direction, const btVector3& normal);
+	btVector3 parallelComponent(const btVector3& direction, const btVector3& normal);
+	btVector3 perpindicularComponent(const btVector3& direction, const btVector3& normal);
+
+	bool recoverFromPenetration(btCollisionWorld * collisionWorld);
+	void stepUp(btCollisionWorld * collisionWorld);
+	void updateTargetPositionBasedOnCollision(const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
+	void stepForwardAndStrafe(btCollisionWorld * collisionWorld, const btVector3& walkMove);
+	void stepDown(btCollisionWorld * collisionWorld, btScalar dt);
+
+	virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1);
 
+	void setUpVector(const btVector3& up);
+
+	btQuaternion getRotation(btVector3 & v0, btVector3 & v1) const;
+
+public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
-	~btKinematicCharacterController ();
-	
+	btKinematicCharacterController(btPairCachingGhostObject * ghostObject, btConvexShape * convexShape, btScalar stepHeight, const btVector3& up = btVector3(1.0, 0.0, 0.0));
+	~btKinematicCharacterController();
 
 	///btActionInterface interface
-	virtual void updateAction( btCollisionWorld* collisionWorld,btScalar deltaTime)
+	virtual void updateAction(btCollisionWorld * collisionWorld, btScalar deltaTime)
 	{
-		preStep ( collisionWorld);
-		playerStep (collisionWorld, deltaTime);
+		preStep(collisionWorld);
+		playerStep(collisionWorld, deltaTime);
 	}
-	
+
 	///btActionInterface interface
-	void	debugDraw(btIDebugDraw* debugDrawer);
+	void debugDraw(btIDebugDraw * debugDrawer);
 
-	void setUpAxis (int axis)
-	{
-		if (axis < 0)
-			axis = 0;
-		if (axis > 2)
-			axis = 2;
-		m_upAxis = axis;
-	}
+	void setUp(const btVector3& up);
+
+	const btVector3& getUp() { return m_up; }
 
 	/// This should probably be called setPositionIncrementPerSimulatorStep.
 	/// This is neither a direction nor a velocity, but the amount to
 	///	increment the position each simulation iteration, regardless
 	///	of dt.
 	/// This call will reset any velocity set by setVelocityForTimeInterval().
-	virtual void	setWalkDirection(const btVector3& walkDirection);
+	virtual void setWalkDirection(const btVector3& walkDirection);
 
 	/// Caller provides a velocity with which the character should move for
 	///	the given time period.  After the time period, velocity is reset
@@ -134,37 +144,57 @@ public:
 	/// This call will reset any walk direction set by setWalkDirection().
 	/// Negative time intervals will result in no motion.
 	virtual void setVelocityForTimeInterval(const btVector3& velocity,
-				btScalar timeInterval);
+											btScalar timeInterval);
+
+	virtual void setAngularVelocity(const btVector3& velocity);
+	virtual const btVector3& getAngularVelocity() const;
+
+	virtual void setLinearVelocity(const btVector3& velocity);
+	virtual btVector3 getLinearVelocity() const;
 
-	void reset ( btCollisionWorld* collisionWorld );
-	void warp (const btVector3& origin);
+	void setLinearDamping(btScalar d) { m_linearDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
+	btScalar getLinearDamping() const { return m_linearDamping; }
+	void setAngularDamping(btScalar d) { m_angularDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); }
+	btScalar getAngularDamping() const { return m_angularDamping; }
 
-	void preStep (  btCollisionWorld* collisionWorld);
-	void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
+	void reset(btCollisionWorld * collisionWorld);
+	void warp(const btVector3& origin);
 
-	void setFallSpeed (btScalar fallSpeed);
-	void setJumpSpeed (btScalar jumpSpeed);
-	void setMaxJumpHeight (btScalar maxJumpHeight);
-	bool canJump () const;
+	void preStep(btCollisionWorld * collisionWorld);
+	void playerStep(btCollisionWorld * collisionWorld, btScalar dt);
 
-	void jump ();
+	void setStepHeight(btScalar h);
+	btScalar getStepHeight() const { return m_stepHeight; }
+	void setFallSpeed(btScalar fallSpeed);
+	btScalar getFallSpeed() const { return m_fallSpeed; }
+	void setJumpSpeed(btScalar jumpSpeed);
+	btScalar getJumpSpeed() const { return m_jumpSpeed; }
+	void setMaxJumpHeight(btScalar maxJumpHeight);
+	bool canJump() const;
 
-	void setGravity(btScalar gravity);
-	btScalar getGravity() const;
+	void jump(const btVector3& v = btVector3(0, 0, 0));
+
+	void applyImpulse(const btVector3& v) { jump(v); }
+
+	void setGravity(const btVector3& gravity);
+	btVector3 getGravity() const;
 
 	/// The max slope determines the maximum angle that the controller can walk up.
 	/// The slope angle is measured in radians.
 	void setMaxSlope(btScalar slopeRadians);
 	btScalar getMaxSlope() const;
 
+	void setMaxPenetrationDepth(btScalar d);
+	btScalar getMaxPenetrationDepth() const;
+
 	btPairCachingGhostObject* getGhostObject();
-	void	setUseGhostSweepTest(bool useGhostObjectSweepTest)
+	void setUseGhostSweepTest(bool useGhostObjectSweepTest)
 	{
 		m_useGhostObjectSweepTest = useGhostObjectSweepTest;
 	}
 
-	bool onGround () const;
-	void setUpInterpolate (bool value);
+	bool onGround() const;
+	void setUpInterpolate(bool value);
 };
 
-#endif // BT_KINEMATIC_CHARACTER_CONTROLLER_H
+#endif  // BT_KINEMATIC_CHARACTER_CONTROLLER_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp
new file mode 100644
index 00000000000..27f76b8425c
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp
@@ -0,0 +1,1084 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btBatchedConstraints.h"
+
+#include "LinearMath/btIDebugDraw.h"
+#include "LinearMath/btMinMax.h"
+#include "LinearMath/btStackAlloc.h"
+#include "LinearMath/btQuickprof.h"
+
+#include <string.h>  //for memset
+
+#include <cmath>
+
+const int kNoMerge = -1;
+
+bool btBatchedConstraints::s_debugDrawBatches = false;
+
+struct btBatchedConstraintInfo
+{
+	int constraintIndex;
+	int numConstraintRows;
+	int bodyIds[2];
+};
+
+struct btBatchInfo
+{
+	int numConstraints;
+	int mergeIndex;
+
+	btBatchInfo() : numConstraints(0), mergeIndex(kNoMerge) {}
+};
+
+bool btBatchedConstraints::validate(btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies) const
+{
+	//
+	// validate: for debugging only. Verify coloring of bodies, that no body is touched by more than one batch in any given phase
+	//
+	int errors = 0;
+	const int kUnassignedBatch = -1;
+
+	btAlignedObjectArray<int> bodyBatchId;
+	for (int iPhase = 0; iPhase < m_phases.size(); ++iPhase)
+	{
+		bodyBatchId.resizeNoInitialize(0);
+		bodyBatchId.resize(bodies.size(), kUnassignedBatch);
+		const Range& phase = m_phases[iPhase];
+		for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch)
+		{
+			const Range& batch = m_batches[iBatch];
+			for (int iiCons = batch.begin; iiCons < batch.end; ++iiCons)
+			{
+				int iCons = m_constraintIndices[iiCons];
+				const btSolverConstraint& cons = constraints->at(iCons);
+				const btSolverBody& bodyA = bodies[cons.m_solverBodyIdA];
+				const btSolverBody& bodyB = bodies[cons.m_solverBodyIdB];
+				if (!bodyA.internalGetInvMass().isZero())
+				{
+					int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdA];
+					if (thisBodyBatchId == kUnassignedBatch)
+					{
+						bodyBatchId[cons.m_solverBodyIdA] = iBatch;
+					}
+					else if (thisBodyBatchId != iBatch)
+					{
+						btAssert(!"dynamic body is used in 2 different batches in the same phase");
+						errors++;
+					}
+				}
+				if (!bodyB.internalGetInvMass().isZero())
+				{
+					int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdB];
+					if (thisBodyBatchId == kUnassignedBatch)
+					{
+						bodyBatchId[cons.m_solverBodyIdB] = iBatch;
+					}
+					else if (thisBodyBatchId != iBatch)
+					{
+						btAssert(!"dynamic body is used in 2 different batches in the same phase");
+						errors++;
+					}
+				}
+			}
+		}
+	}
+	return errors == 0;
+}
+
+static void debugDrawSingleBatch(const btBatchedConstraints* bc,
+								 btConstraintArray* constraints,
+								 const btAlignedObjectArray<btSolverBody>& bodies,
+								 int iBatch,
+								 const btVector3& color,
+								 const btVector3& offset)
+{
+	if (bc && bc->m_debugDrawer && iBatch < bc->m_batches.size())
+	{
+		const btBatchedConstraints::Range& b = bc->m_batches[iBatch];
+		for (int iiCon = b.begin; iiCon < b.end; ++iiCon)
+		{
+			int iCon = bc->m_constraintIndices[iiCon];
+			const btSolverConstraint& con = constraints->at(iCon);
+			int iBody0 = con.m_solverBodyIdA;
+			int iBody1 = con.m_solverBodyIdB;
+			btVector3 pos0 = bodies[iBody0].getWorldTransform().getOrigin() + offset;
+			btVector3 pos1 = bodies[iBody1].getWorldTransform().getOrigin() + offset;
+			bc->m_debugDrawer->drawLine(pos0, pos1, color);
+		}
+	}
+}
+
+static void debugDrawPhase(const btBatchedConstraints* bc,
+						   btConstraintArray* constraints,
+						   const btAlignedObjectArray<btSolverBody>& bodies,
+						   int iPhase,
+						   const btVector3& color0,
+						   const btVector3& color1,
+						   const btVector3& offset)
+{
+	BT_PROFILE("debugDrawPhase");
+	if (bc && bc->m_debugDrawer && iPhase < bc->m_phases.size())
+	{
+		const btBatchedConstraints::Range& phase = bc->m_phases[iPhase];
+		for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch)
+		{
+			float tt = float(iBatch - phase.begin) / float(btMax(1, phase.end - phase.begin - 1));
+			btVector3 col = lerp(color0, color1, tt);
+			debugDrawSingleBatch(bc, constraints, bodies, iBatch, col, offset);
+		}
+	}
+}
+
+static void debugDrawAllBatches(const btBatchedConstraints* bc,
+								btConstraintArray* constraints,
+								const btAlignedObjectArray<btSolverBody>& bodies)
+{
+	BT_PROFILE("debugDrawAllBatches");
+	if (bc && bc->m_debugDrawer && bc->m_phases.size() > 0)
+	{
+		btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
+		btVector3 bboxMax = -bboxMin;
+		for (int iBody = 0; iBody < bodies.size(); ++iBody)
+		{
+			const btVector3& pos = bodies[iBody].getWorldTransform().getOrigin();
+			bboxMin.setMin(pos);
+			bboxMax.setMax(pos);
+		}
+		btVector3 bboxExtent = bboxMax - bboxMin;
+		btVector3 offsetBase = btVector3(0, bboxExtent.y() * 1.1f, 0);
+		btVector3 offsetStep = btVector3(0, 0, bboxExtent.z() * 1.1f);
+		int numPhases = bc->m_phases.size();
+		for (int iPhase = 0; iPhase < numPhases; ++iPhase)
+		{
+			float b = float(iPhase) / float(numPhases - 1);
+			btVector3 color0 = btVector3(1, 0, b);
+			btVector3 color1 = btVector3(0, 1, b);
+			btVector3 offset = offsetBase + offsetStep * (float(iPhase) - float(numPhases - 1) * 0.5);
+			debugDrawPhase(bc, constraints, bodies, iPhase, color0, color1, offset);
+		}
+	}
+}
+
+static void initBatchedBodyDynamicFlags(btAlignedObjectArray<bool>* outBodyDynamicFlags, const btAlignedObjectArray<btSolverBody>& bodies)
+{
+	BT_PROFILE("initBatchedBodyDynamicFlags");
+	btAlignedObjectArray<bool>& bodyDynamicFlags = *outBodyDynamicFlags;
+	bodyDynamicFlags.resizeNoInitialize(bodies.size());
+	for (int i = 0; i < bodies.size(); ++i)
+	{
+		const btSolverBody& body = bodies[i];
+		bodyDynamicFlags[i] = (body.internalGetInvMass().x() > btScalar(0));
+	}
+}
+
+static int runLengthEncodeConstraintInfo(btBatchedConstraintInfo* outConInfos, int numConstraints)
+{
+	BT_PROFILE("runLengthEncodeConstraintInfo");
+	// detect and run-length encode constraint rows that repeat the same bodies
+	int iDest = 0;
+	int iSrc = 0;
+	while (iSrc < numConstraints)
+	{
+		const btBatchedConstraintInfo& srcConInfo = outConInfos[iSrc];
+		btBatchedConstraintInfo& conInfo = outConInfos[iDest];
+		conInfo.constraintIndex = iSrc;
+		conInfo.bodyIds[0] = srcConInfo.bodyIds[0];
+		conInfo.bodyIds[1] = srcConInfo.bodyIds[1];
+		while (iSrc < numConstraints && outConInfos[iSrc].bodyIds[0] == srcConInfo.bodyIds[0] && outConInfos[iSrc].bodyIds[1] == srcConInfo.bodyIds[1])
+		{
+			++iSrc;
+		}
+		conInfo.numConstraintRows = iSrc - conInfo.constraintIndex;
+		++iDest;
+	}
+	return iDest;
+}
+
+struct ReadSolverConstraintsLoop : public btIParallelForBody
+{
+	btBatchedConstraintInfo* m_outConInfos;
+	btConstraintArray* m_constraints;
+
+	ReadSolverConstraintsLoop(btBatchedConstraintInfo* outConInfos, btConstraintArray* constraints)
+	{
+		m_outConInfos = outConInfos;
+		m_constraints = constraints;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		for (int i = iBegin; i < iEnd; ++i)
+		{
+			btBatchedConstraintInfo& conInfo = m_outConInfos[i];
+			const btSolverConstraint& con = m_constraints->at(i);
+			conInfo.bodyIds[0] = con.m_solverBodyIdA;
+			conInfo.bodyIds[1] = con.m_solverBodyIdB;
+			conInfo.constraintIndex = i;
+			conInfo.numConstraintRows = 1;
+		}
+	}
+};
+
+static int initBatchedConstraintInfo(btBatchedConstraintInfo* outConInfos, btConstraintArray* constraints)
+{
+	BT_PROFILE("initBatchedConstraintInfo");
+	int numConstraints = constraints->size();
+	bool inParallel = true;
+	if (inParallel)
+	{
+		ReadSolverConstraintsLoop loop(outConInfos, constraints);
+		int grainSize = 1200;
+		btParallelFor(0, numConstraints, grainSize, loop);
+	}
+	else
+	{
+		for (int i = 0; i < numConstraints; ++i)
+		{
+			btBatchedConstraintInfo& conInfo = outConInfos[i];
+			const btSolverConstraint& con = constraints->at(i);
+			conInfo.bodyIds[0] = con.m_solverBodyIdA;
+			conInfo.bodyIds[1] = con.m_solverBodyIdB;
+			conInfo.constraintIndex = i;
+			conInfo.numConstraintRows = 1;
+		}
+	}
+	bool useRunLengthEncoding = true;
+	if (useRunLengthEncoding)
+	{
+		numConstraints = runLengthEncodeConstraintInfo(outConInfos, numConstraints);
+	}
+	return numConstraints;
+}
+
+static void expandConstraintRowsInPlace(int* constraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
+{
+	BT_PROFILE("expandConstraintRowsInPlace");
+	if (numConstraintRows > numConstraints)
+	{
+		// we walk the array in reverse to avoid overwriteing
+		for (int iCon = numConstraints - 1; iCon >= 0; --iCon)
+		{
+			const btBatchedConstraintInfo& conInfo = conInfos[iCon];
+			int iBatch = constraintBatchIds[iCon];
+			for (int i = conInfo.numConstraintRows - 1; i >= 0; --i)
+			{
+				int iDest = conInfo.constraintIndex + i;
+				btAssert(iDest >= iCon);
+				btAssert(iDest >= 0 && iDest < numConstraintRows);
+				constraintBatchIds[iDest] = iBatch;
+			}
+		}
+	}
+}
+
+static void expandConstraintRows(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
+{
+	BT_PROFILE("expandConstraintRows");
+	for (int iCon = 0; iCon < numConstraints; ++iCon)
+	{
+		const btBatchedConstraintInfo& conInfo = conInfos[iCon];
+		int iBatch = srcConstraintBatchIds[iCon];
+		for (int i = 0; i < conInfo.numConstraintRows; ++i)
+		{
+			int iDest = conInfo.constraintIndex + i;
+			btAssert(iDest >= iCon);
+			btAssert(iDest >= 0 && iDest < numConstraintRows);
+			destConstraintBatchIds[iDest] = iBatch;
+		}
+	}
+}
+
+struct ExpandConstraintRowsLoop : public btIParallelForBody
+{
+	int* m_destConstraintBatchIds;
+	const int* m_srcConstraintBatchIds;
+	const btBatchedConstraintInfo* m_conInfos;
+	int m_numConstraintRows;
+
+	ExpandConstraintRowsLoop(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraintRows)
+	{
+		m_destConstraintBatchIds = destConstraintBatchIds;
+		m_srcConstraintBatchIds = srcConstraintBatchIds;
+		m_conInfos = conInfos;
+		m_numConstraintRows = numConstraintRows;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		expandConstraintRows(m_destConstraintBatchIds, m_srcConstraintBatchIds + iBegin, m_conInfos + iBegin, iEnd - iBegin, m_numConstraintRows);
+	}
+};
+
+static void expandConstraintRowsMt(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
+{
+	BT_PROFILE("expandConstraintRowsMt");
+	ExpandConstraintRowsLoop loop(destConstraintBatchIds, srcConstraintBatchIds, conInfos, numConstraintRows);
+	int grainSize = 600;
+	btParallelFor(0, numConstraints, grainSize, loop);
+}
+
+static void initBatchedConstraintInfoArray(btAlignedObjectArray<btBatchedConstraintInfo>* outConInfos, btConstraintArray* constraints)
+{
+	BT_PROFILE("initBatchedConstraintInfoArray");
+	btAlignedObjectArray<btBatchedConstraintInfo>& conInfos = *outConInfos;
+	int numConstraints = constraints->size();
+	conInfos.resizeNoInitialize(numConstraints);
+
+	int newSize = initBatchedConstraintInfo(&outConInfos->at(0), constraints);
+	conInfos.resizeNoInitialize(newSize);
+}
+
+static void mergeSmallBatches(btBatchInfo* batches, int iBeginBatch, int iEndBatch, int minBatchSize, int maxBatchSize)
+{
+	BT_PROFILE("mergeSmallBatches");
+	for (int iBatch = iEndBatch - 1; iBatch >= iBeginBatch; --iBatch)
+	{
+		btBatchInfo& batch = batches[iBatch];
+		if (batch.mergeIndex == kNoMerge && batch.numConstraints > 0 && batch.numConstraints < minBatchSize)
+		{
+			for (int iDestBatch = iBatch - 1; iDestBatch >= iBeginBatch; --iDestBatch)
+			{
+				btBatchInfo& destBatch = batches[iDestBatch];
+				if (destBatch.mergeIndex == kNoMerge && (destBatch.numConstraints + batch.numConstraints) < maxBatchSize)
+				{
+					destBatch.numConstraints += batch.numConstraints;
+					batch.numConstraints = 0;
+					batch.mergeIndex = iDestBatch;
+					break;
+				}
+			}
+		}
+	}
+	// flatten mergeIndexes
+	// e.g. in case where A was merged into B and then B was merged into C, we need A to point to C instead of B
+	// Note: loop goes forward through batches because batches always merge from higher indexes to lower,
+	//     so by going from low to high it reduces the amount of trail-following
+	for (int iBatch = iBeginBatch; iBatch < iEndBatch; ++iBatch)
+	{
+		btBatchInfo& batch = batches[iBatch];
+		if (batch.mergeIndex != kNoMerge)
+		{
+			int iMergeDest = batches[batch.mergeIndex].mergeIndex;
+			// follow trail of merges to the end
+			while (iMergeDest != kNoMerge)
+			{
+				int iNext = batches[iMergeDest].mergeIndex;
+				if (iNext == kNoMerge)
+				{
+					batch.mergeIndex = iMergeDest;
+					break;
+				}
+				iMergeDest = iNext;
+			}
+		}
+	}
+}
+
+static void updateConstraintBatchIdsForMerges(int* constraintBatchIds, int numConstraints, const btBatchInfo* batches, int numBatches)
+{
+	BT_PROFILE("updateConstraintBatchIdsForMerges");
+	// update batchIds to account for merges
+	for (int i = 0; i < numConstraints; ++i)
+	{
+		int iBatch = constraintBatchIds[i];
+		btAssert(iBatch < numBatches);
+		// if this constraint references a batch that was merged into another batch
+		if (batches[iBatch].mergeIndex != kNoMerge)
+		{
+			// update batchId
+			constraintBatchIds[i] = batches[iBatch].mergeIndex;
+		}
+	}
+}
+
+struct UpdateConstraintBatchIdsForMergesLoop : public btIParallelForBody
+{
+	int* m_constraintBatchIds;
+	const btBatchInfo* m_batches;
+	int m_numBatches;
+
+	UpdateConstraintBatchIdsForMergesLoop(int* constraintBatchIds, const btBatchInfo* batches, int numBatches)
+	{
+		m_constraintBatchIds = constraintBatchIds;
+		m_batches = batches;
+		m_numBatches = numBatches;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("UpdateConstraintBatchIdsForMergesLoop");
+		updateConstraintBatchIdsForMerges(m_constraintBatchIds + iBegin, iEnd - iBegin, m_batches, m_numBatches);
+	}
+};
+
+static void updateConstraintBatchIdsForMergesMt(int* constraintBatchIds, int numConstraints, const btBatchInfo* batches, int numBatches)
+{
+	BT_PROFILE("updateConstraintBatchIdsForMergesMt");
+	UpdateConstraintBatchIdsForMergesLoop loop(constraintBatchIds, batches, numBatches);
+	int grainSize = 800;
+	btParallelFor(0, numConstraints, grainSize, loop);
+}
+
+inline bool BatchCompare(const btBatchedConstraints::Range& a, const btBatchedConstraints::Range& b)
+{
+	int lenA = a.end - a.begin;
+	int lenB = b.end - b.begin;
+	return lenA > lenB;
+}
+
+static void writeOutConstraintIndicesForRangeOfBatches(btBatchedConstraints* bc,
+													   const int* constraintBatchIds,
+													   int numConstraints,
+													   int* constraintIdPerBatch,
+													   int batchBegin,
+													   int batchEnd)
+{
+	BT_PROFILE("writeOutConstraintIndicesForRangeOfBatches");
+	for (int iCon = 0; iCon < numConstraints; ++iCon)
+	{
+		int iBatch = constraintBatchIds[iCon];
+		if (iBatch >= batchBegin && iBatch < batchEnd)
+		{
+			int iDestCon = constraintIdPerBatch[iBatch];
+			constraintIdPerBatch[iBatch] = iDestCon + 1;
+			bc->m_constraintIndices[iDestCon] = iCon;
+		}
+	}
+}
+
+struct WriteOutConstraintIndicesLoop : public btIParallelForBody
+{
+	btBatchedConstraints* m_batchedConstraints;
+	const int* m_constraintBatchIds;
+	int m_numConstraints;
+	int* m_constraintIdPerBatch;
+	int m_maxNumBatchesPerPhase;
+
+	WriteOutConstraintIndicesLoop(btBatchedConstraints* bc, const int* constraintBatchIds, int numConstraints, int* constraintIdPerBatch, int maxNumBatchesPerPhase)
+	{
+		m_batchedConstraints = bc;
+		m_constraintBatchIds = constraintBatchIds;
+		m_numConstraints = numConstraints;
+		m_constraintIdPerBatch = constraintIdPerBatch;
+		m_maxNumBatchesPerPhase = maxNumBatchesPerPhase;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("WriteOutConstraintIndicesLoop");
+		int batchBegin = iBegin * m_maxNumBatchesPerPhase;
+		int batchEnd = iEnd * m_maxNumBatchesPerPhase;
+		writeOutConstraintIndicesForRangeOfBatches(m_batchedConstraints,
+												   m_constraintBatchIds,
+												   m_numConstraints,
+												   m_constraintIdPerBatch,
+												   batchBegin,
+												   batchEnd);
+	}
+};
+
+static void writeOutConstraintIndicesMt(btBatchedConstraints* bc,
+										const int* constraintBatchIds,
+										int numConstraints,
+										int* constraintIdPerBatch,
+										int maxNumBatchesPerPhase,
+										int numPhases)
+{
+	BT_PROFILE("writeOutConstraintIndicesMt");
+	bool inParallel = true;
+	if (inParallel)
+	{
+		WriteOutConstraintIndicesLoop loop(bc, constraintBatchIds, numConstraints, constraintIdPerBatch, maxNumBatchesPerPhase);
+		btParallelFor(0, numPhases, 1, loop);
+	}
+	else
+	{
+		for (int iCon = 0; iCon < numConstraints; ++iCon)
+		{
+			int iBatch = constraintBatchIds[iCon];
+			int iDestCon = constraintIdPerBatch[iBatch];
+			constraintIdPerBatch[iBatch] = iDestCon + 1;
+			bc->m_constraintIndices[iDestCon] = iCon;
+		}
+	}
+}
+
+static void writeGrainSizes(btBatchedConstraints* bc)
+{
+	typedef btBatchedConstraints::Range Range;
+	int numPhases = bc->m_phases.size();
+	bc->m_phaseGrainSize.resizeNoInitialize(numPhases);
+	int numThreads = btGetTaskScheduler()->getNumThreads();
+	for (int iPhase = 0; iPhase < numPhases; ++iPhase)
+	{
+		const Range& phase = bc->m_phases[iPhase];
+		int numBatches = phase.end - phase.begin;
+		float grainSize = std::floor((0.25f * numBatches / float(numThreads)) + 0.0f);
+		bc->m_phaseGrainSize[iPhase] = btMax(1, int(grainSize));
+	}
+}
+
+static void writeOutBatches(btBatchedConstraints* bc,
+							const int* constraintBatchIds,
+							int numConstraints,
+							const btBatchInfo* batches,
+							int* batchWork,
+							int maxNumBatchesPerPhase,
+							int numPhases)
+{
+	BT_PROFILE("writeOutBatches");
+	typedef btBatchedConstraints::Range Range;
+	bc->m_constraintIndices.reserve(numConstraints);
+	bc->m_batches.resizeNoInitialize(0);
+	bc->m_phases.resizeNoInitialize(0);
+
+	//int maxNumBatches = numPhases * maxNumBatchesPerPhase;
+	{
+		int* constraintIdPerBatch = batchWork;  // for each batch, keep an index into the next available slot in the m_constraintIndices array
+		int iConstraint = 0;
+		for (int iPhase = 0; iPhase < numPhases; ++iPhase)
+		{
+			int curPhaseBegin = bc->m_batches.size();
+			int iBegin = iPhase * maxNumBatchesPerPhase;
+			int iEnd = iBegin + maxNumBatchesPerPhase;
+			for (int i = iBegin; i < iEnd; ++i)
+			{
+				const btBatchInfo& batch = batches[i];
+				int curBatchBegin = iConstraint;
+				constraintIdPerBatch[i] = curBatchBegin;  // record the start of each batch in m_constraintIndices array
+				int numConstraints = batch.numConstraints;
+				iConstraint += numConstraints;
+				if (numConstraints > 0)
+				{
+					bc->m_batches.push_back(Range(curBatchBegin, iConstraint));
+				}
+			}
+			// if any batches were emitted this phase,
+			if (bc->m_batches.size() > curPhaseBegin)
+			{
+				// output phase
+				bc->m_phases.push_back(Range(curPhaseBegin, bc->m_batches.size()));
+			}
+		}
+
+		btAssert(iConstraint == numConstraints);
+		bc->m_constraintIndices.resizeNoInitialize(numConstraints);
+		writeOutConstraintIndicesMt(bc, constraintBatchIds, numConstraints, constraintIdPerBatch, maxNumBatchesPerPhase, numPhases);
+	}
+	// for each phase
+	for (int iPhase = 0; iPhase < bc->m_phases.size(); ++iPhase)
+	{
+		// sort the batches from largest to smallest (can be helpful to some task schedulers)
+		const Range& curBatches = bc->m_phases[iPhase];
+		bc->m_batches.quickSortInternal(BatchCompare, curBatches.begin, curBatches.end - 1);
+	}
+	bc->m_phaseOrder.resize(bc->m_phases.size());
+	for (int i = 0; i < bc->m_phases.size(); ++i)
+	{
+		bc->m_phaseOrder[i] = i;
+	}
+	writeGrainSizes(bc);
+}
+
+//
+// PreallocatedMemoryHelper -- helper object for allocating a number of chunks of memory in a single contiguous block.
+//                             It is generally more efficient to do a single larger allocation than many smaller allocations.
+//
+// Example Usage:
+//
+//  btVector3* bodyPositions = NULL;
+//  btBatchedConstraintInfo* conInfos = NULL;
+//  {
+//    PreallocatedMemoryHelper<8> memHelper;
+//    memHelper.addChunk( (void**) &bodyPositions, sizeof( btVector3 ) * bodies.size() );
+//    memHelper.addChunk( (void**) &conInfos, sizeof( btBatchedConstraintInfo ) * numConstraints );
+//    void* memPtr = malloc( memHelper.getSizeToAllocate() );  // allocate the memory
+//    memHelper.setChunkPointers( memPtr );  // update pointers to chunks
+//  }
+template <int N>
+class PreallocatedMemoryHelper
+{
+	struct Chunk
+	{
+		void** ptr;
+		size_t size;
+	};
+	Chunk m_chunks[N];
+	int m_numChunks;
+
+public:
+	PreallocatedMemoryHelper() { m_numChunks = 0; }
+	void addChunk(void** ptr, size_t sz)
+	{
+		btAssert(m_numChunks < N);
+		if (m_numChunks < N)
+		{
+			Chunk& chunk = m_chunks[m_numChunks];
+			chunk.ptr = ptr;
+			chunk.size = sz;
+			m_numChunks++;
+		}
+	}
+	size_t getSizeToAllocate() const
+	{
+		size_t totalSize = 0;
+		for (int i = 0; i < m_numChunks; ++i)
+		{
+			totalSize += m_chunks[i].size;
+		}
+		return totalSize;
+	}
+	void setChunkPointers(void* mem) const
+	{
+		size_t totalSize = 0;
+		for (int i = 0; i < m_numChunks; ++i)
+		{
+			const Chunk& chunk = m_chunks[i];
+			char* chunkPtr = static_cast<char*>(mem) + totalSize;
+			*chunk.ptr = chunkPtr;
+			totalSize += chunk.size;
+		}
+	}
+};
+
+static btVector3 findMaxDynamicConstraintExtent(
+	btVector3* bodyPositions,
+	bool* bodyDynamicFlags,
+	btBatchedConstraintInfo* conInfos,
+	int numConstraints,
+	int numBodies)
+{
+	BT_PROFILE("findMaxDynamicConstraintExtent");
+	btVector3 consExtent = btVector3(1, 1, 1) * 0.001;
+	for (int iCon = 0; iCon < numConstraints; ++iCon)
+	{
+		const btBatchedConstraintInfo& con = conInfos[iCon];
+		int iBody0 = con.bodyIds[0];
+		int iBody1 = con.bodyIds[1];
+		btAssert(iBody0 >= 0 && iBody0 < numBodies);
+		btAssert(iBody1 >= 0 && iBody1 < numBodies);
+		// is it a dynamic constraint?
+		if (bodyDynamicFlags[iBody0] && bodyDynamicFlags[iBody1])
+		{
+			btVector3 delta = bodyPositions[iBody1] - bodyPositions[iBody0];
+			consExtent.setMax(delta.absolute());
+		}
+	}
+	return consExtent;
+}
+
+struct btIntVec3
+{
+	int m_ints[3];
+
+	SIMD_FORCE_INLINE const int& operator[](int i) const { return m_ints[i]; }
+	SIMD_FORCE_INLINE int& operator[](int i) { return m_ints[i]; }
+};
+
+struct AssignConstraintsToGridBatchesParams
+{
+	bool* bodyDynamicFlags;
+	btIntVec3* bodyGridCoords;
+	int numBodies;
+	btBatchedConstraintInfo* conInfos;
+	int* constraintBatchIds;
+	btIntVec3 gridChunkDim;
+	int maxNumBatchesPerPhase;
+	int numPhases;
+	int phaseMask;
+
+	AssignConstraintsToGridBatchesParams()
+	{
+		memset(this, 0, sizeof(*this));
+	}
+};
+
+static void assignConstraintsToGridBatches(const AssignConstraintsToGridBatchesParams& params, int iConBegin, int iConEnd)
+{
+	BT_PROFILE("assignConstraintsToGridBatches");
+	// (can be done in parallel)
+	for (int iCon = iConBegin; iCon < iConEnd; ++iCon)
+	{
+		const btBatchedConstraintInfo& con = params.conInfos[iCon];
+		int iBody0 = con.bodyIds[0];
+		int iBody1 = con.bodyIds[1];
+		int iPhase = iCon;  //iBody0; // pseudorandom choice to distribute evenly amongst phases
+		iPhase &= params.phaseMask;
+		int gridCoord[3];
+		// is it a dynamic constraint?
+		if (params.bodyDynamicFlags[iBody0] && params.bodyDynamicFlags[iBody1])
+		{
+			const btIntVec3& body0Coords = params.bodyGridCoords[iBody0];
+			const btIntVec3& body1Coords = params.bodyGridCoords[iBody1];
+			// for each dimension x,y,z,
+			for (int i = 0; i < 3; ++i)
+			{
+				int coordMin = btMin(body0Coords.m_ints[i], body1Coords.m_ints[i]);
+				int coordMax = btMax(body0Coords.m_ints[i], body1Coords.m_ints[i]);
+				if (coordMin != coordMax)
+				{
+					btAssert(coordMax == coordMin + 1);
+					if ((coordMin & 1) == 0)
+					{
+						iPhase &= ~(1 << i);  // force bit off
+					}
+					else
+					{
+						iPhase |= (1 << i);  // force bit on
+						iPhase &= params.phaseMask;
+					}
+				}
+				gridCoord[i] = coordMin;
+			}
+		}
+		else
+		{
+			if (!params.bodyDynamicFlags[iBody0])
+			{
+				iBody0 = con.bodyIds[1];
+			}
+			btAssert(params.bodyDynamicFlags[iBody0]);
+			const btIntVec3& body0Coords = params.bodyGridCoords[iBody0];
+			// for each dimension x,y,z,
+			for (int i = 0; i < 3; ++i)
+			{
+				gridCoord[i] = body0Coords.m_ints[i];
+			}
+		}
+		// calculate chunk coordinates
+		int chunkCoord[3];
+		btIntVec3 gridChunkDim = params.gridChunkDim;
+		// for each dimension x,y,z,
+		for (int i = 0; i < 3; ++i)
+		{
+			int coordOffset = (iPhase >> i) & 1;
+			chunkCoord[i] = (gridCoord[i] - coordOffset) / 2;
+			btClamp(chunkCoord[i], 0, gridChunkDim[i] - 1);
+			btAssert(chunkCoord[i] < gridChunkDim[i]);
+		}
+		int iBatch = iPhase * params.maxNumBatchesPerPhase + chunkCoord[0] + chunkCoord[1] * gridChunkDim[0] + chunkCoord[2] * gridChunkDim[0] * gridChunkDim[1];
+		btAssert(iBatch >= 0 && iBatch < params.maxNumBatchesPerPhase * params.numPhases);
+		params.constraintBatchIds[iCon] = iBatch;
+	}
+}
+
+struct AssignConstraintsToGridBatchesLoop : public btIParallelForBody
+{
+	const AssignConstraintsToGridBatchesParams* m_params;
+
+	AssignConstraintsToGridBatchesLoop(const AssignConstraintsToGridBatchesParams& params)
+	{
+		m_params = &params;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		assignConstraintsToGridBatches(*m_params, iBegin, iEnd);
+	}
+};
+
+//
+// setupSpatialGridBatchesMt -- generate batches using a uniform 3D grid
+//
+/*
+
+Bodies are treated as 3D points at their center of mass. We only consider dynamic bodies at this stage,
+because only dynamic bodies are mutated when a constraint is solved, thus subject to race conditions.
+
+1. Compute a bounding box around all dynamic bodies
+2. Compute the maximum extent of all dynamic constraints. Each dynamic constraint is treated as a line segment, and we need the size of
+   box that will fully enclose any single dynamic constraint
+
+3. Establish the cell size of our grid, the cell size in each dimension must be at least as large as the dynamic constraints max-extent,
+   so that no dynamic constraint can span more than 2 cells of our grid on any axis of the grid. The cell size should be adjusted
+   larger in order to keep the total number of cells from being excessively high
+
+Key idea: Given that each constraint spans 1 or 2 grid cells in each dimension, we can handle all constraints by processing
+          in chunks of 2x2x2 cells with 8 different 1-cell offsets ((0,0,0),(0,0,1),(0,1,0),(0,1,1),(1,0,0)...).
+          For each of the 8 offsets, we create a phase, and for each 2x2x2 chunk with dynamic constraints becomes a batch in that phase.
+
+4. Once the grid is established, we can calculate for each constraint which phase and batch it belongs in.
+
+5. Do a merge small batches on the batches of each phase separately, to try to even out the sizes of batches
+
+Optionally, we can "collapse" one dimension of our 3D grid to turn it into a 2D grid, which reduces the number of phases
+to 4. With fewer phases, there are more constraints per phase and this makes it easier to create batches of a useful size.
+*/
+//
+static void setupSpatialGridBatchesMt(
+	btBatchedConstraints* batchedConstraints,
+	btAlignedObjectArray<char>* scratchMemory,
+	btConstraintArray* constraints,
+	const btAlignedObjectArray<btSolverBody>& bodies,
+	int minBatchSize,
+	int maxBatchSize,
+	bool use2DGrid)
+{
+	BT_PROFILE("setupSpatialGridBatchesMt");
+	const int numPhases = 8;
+	int numConstraints = constraints->size();
+	int numConstraintRows = constraints->size();
+
+	const int maxGridChunkCount = 128;
+	int allocNumBatchesPerPhase = maxGridChunkCount;
+	int minNumBatchesPerPhase = 16;
+	int allocNumBatches = allocNumBatchesPerPhase * numPhases;
+
+	btVector3* bodyPositions = NULL;
+	bool* bodyDynamicFlags = NULL;
+	btIntVec3* bodyGridCoords = NULL;
+	btBatchInfo* batches = NULL;
+	int* batchWork = NULL;
+	btBatchedConstraintInfo* conInfos = NULL;
+	int* constraintBatchIds = NULL;
+	int* constraintRowBatchIds = NULL;
+	{
+		PreallocatedMemoryHelper<10> memHelper;
+		memHelper.addChunk((void**)&bodyPositions, sizeof(btVector3) * bodies.size());
+		memHelper.addChunk((void**)&bodyDynamicFlags, sizeof(bool) * bodies.size());
+		memHelper.addChunk((void**)&bodyGridCoords, sizeof(btIntVec3) * bodies.size());
+		memHelper.addChunk((void**)&batches, sizeof(btBatchInfo) * allocNumBatches);
+		memHelper.addChunk((void**)&batchWork, sizeof(int) * allocNumBatches);
+		memHelper.addChunk((void**)&conInfos, sizeof(btBatchedConstraintInfo) * numConstraints);
+		memHelper.addChunk((void**)&constraintBatchIds, sizeof(int) * numConstraints);
+		memHelper.addChunk((void**)&constraintRowBatchIds, sizeof(int) * numConstraintRows);
+		size_t scratchSize = memHelper.getSizeToAllocate();
+		// if we need to reallocate
+		if (scratchMemory->capacity() < scratchSize)
+		{
+			// allocate 6.25% extra to avoid repeated reallocs
+			scratchMemory->reserve(scratchSize + scratchSize / 16);
+		}
+		scratchMemory->resizeNoInitialize(scratchSize);
+		char* memPtr = &scratchMemory->at(0);
+		memHelper.setChunkPointers(memPtr);
+	}
+
+	numConstraints = initBatchedConstraintInfo(conInfos, constraints);
+
+	// compute bounding box around all dynamic bodies
+	// (could be done in parallel)
+	btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
+	btVector3 bboxMax = -bboxMin;
+	//int dynamicBodyCount = 0;
+	for (int i = 0; i < bodies.size(); ++i)
+	{
+		const btSolverBody& body = bodies[i];
+		btVector3 bodyPos = body.getWorldTransform().getOrigin();
+		bool isDynamic = (body.internalGetInvMass().x() > btScalar(0));
+		bodyPositions[i] = bodyPos;
+		bodyDynamicFlags[i] = isDynamic;
+		if (isDynamic)
+		{
+			//dynamicBodyCount++;
+			bboxMin.setMin(bodyPos);
+			bboxMax.setMax(bodyPos);
+		}
+	}
+
+	// find max extent of all dynamic constraints
+	// (could be done in parallel)
+	btVector3 consExtent = findMaxDynamicConstraintExtent(bodyPositions, bodyDynamicFlags, conInfos, numConstraints, bodies.size());
+
+	btVector3 gridExtent = bboxMax - bboxMin;
+
+	gridExtent.setMax(btVector3(btScalar(1), btScalar(1), btScalar(1)));
+
+	btVector3 gridCellSize = consExtent;
+	int gridDim[3];
+	gridDim[0] = int(1.0 + gridExtent.x() / gridCellSize.x());
+	gridDim[1] = int(1.0 + gridExtent.y() / gridCellSize.y());
+	gridDim[2] = int(1.0 + gridExtent.z() / gridCellSize.z());
+
+	// if we can collapse an axis, it will cut our number of phases in half which could be more efficient
+	int phaseMask = 7;
+	bool collapseAxis = use2DGrid;
+	if (collapseAxis)
+	{
+		// pick the smallest axis to collapse, leaving us with the greatest number of cells in our grid
+		int iAxisToCollapse = 0;
+		int axisDim = gridDim[iAxisToCollapse];
+		//for each dimension
+		for (int i = 0; i < 3; ++i)
+		{
+			if (gridDim[i] < axisDim)
+			{
+				iAxisToCollapse = i;
+				axisDim = gridDim[i];
+			}
+		}
+		// collapse it
+		gridCellSize[iAxisToCollapse] = gridExtent[iAxisToCollapse] * 2.0f;
+		phaseMask &= ~(1 << iAxisToCollapse);
+	}
+
+	int numGridChunks = 0;
+	btIntVec3 gridChunkDim;  // each chunk is 2x2x2 group of cells
+	while (true)
+	{
+		gridDim[0] = int(1.0 + gridExtent.x() / gridCellSize.x());
+		gridDim[1] = int(1.0 + gridExtent.y() / gridCellSize.y());
+		gridDim[2] = int(1.0 + gridExtent.z() / gridCellSize.z());
+		gridChunkDim[0] = btMax(1, (gridDim[0] + 0) / 2);
+		gridChunkDim[1] = btMax(1, (gridDim[1] + 0) / 2);
+		gridChunkDim[2] = btMax(1, (gridDim[2] + 0) / 2);
+		numGridChunks = gridChunkDim[0] * gridChunkDim[1] * gridChunkDim[2];
+		float nChunks = float(gridChunkDim[0]) * float(gridChunkDim[1]) * float(gridChunkDim[2]);  // suceptible to integer overflow
+		if (numGridChunks <= maxGridChunkCount && nChunks <= maxGridChunkCount)
+		{
+			break;
+		}
+		gridCellSize *= 1.25;  // should roughly cut numCells in half
+	}
+	btAssert(numGridChunks <= maxGridChunkCount);
+	int maxNumBatchesPerPhase = numGridChunks;
+
+	// for each dynamic body, compute grid coords
+	btVector3 invGridCellSize = btVector3(1, 1, 1) / gridCellSize;
+	// (can be done in parallel)
+	for (int iBody = 0; iBody < bodies.size(); ++iBody)
+	{
+		btIntVec3& coords = bodyGridCoords[iBody];
+		if (bodyDynamicFlags[iBody])
+		{
+			btVector3 v = (bodyPositions[iBody] - bboxMin) * invGridCellSize;
+			coords.m_ints[0] = int(v.x());
+			coords.m_ints[1] = int(v.y());
+			coords.m_ints[2] = int(v.z());
+			btAssert(coords.m_ints[0] >= 0 && coords.m_ints[0] < gridDim[0]);
+			btAssert(coords.m_ints[1] >= 0 && coords.m_ints[1] < gridDim[1]);
+			btAssert(coords.m_ints[2] >= 0 && coords.m_ints[2] < gridDim[2]);
+		}
+		else
+		{
+			coords.m_ints[0] = -1;
+			coords.m_ints[1] = -1;
+			coords.m_ints[2] = -1;
+		}
+	}
+
+	for (int iPhase = 0; iPhase < numPhases; ++iPhase)
+	{
+		int batchBegin = iPhase * maxNumBatchesPerPhase;
+		int batchEnd = batchBegin + maxNumBatchesPerPhase;
+		for (int iBatch = batchBegin; iBatch < batchEnd; ++iBatch)
+		{
+			btBatchInfo& batch = batches[iBatch];
+			batch = btBatchInfo();
+		}
+	}
+
+	{
+		AssignConstraintsToGridBatchesParams params;
+		params.bodyDynamicFlags = bodyDynamicFlags;
+		params.bodyGridCoords = bodyGridCoords;
+		params.numBodies = bodies.size();
+		params.conInfos = conInfos;
+		params.constraintBatchIds = constraintBatchIds;
+		params.gridChunkDim = gridChunkDim;
+		params.maxNumBatchesPerPhase = maxNumBatchesPerPhase;
+		params.numPhases = numPhases;
+		params.phaseMask = phaseMask;
+		bool inParallel = true;
+		if (inParallel)
+		{
+			AssignConstraintsToGridBatchesLoop loop(params);
+			int grainSize = 250;
+			btParallelFor(0, numConstraints, grainSize, loop);
+		}
+		else
+		{
+			assignConstraintsToGridBatches(params, 0, numConstraints);
+		}
+	}
+	for (int iCon = 0; iCon < numConstraints; ++iCon)
+	{
+		const btBatchedConstraintInfo& con = conInfos[iCon];
+		int iBatch = constraintBatchIds[iCon];
+		btBatchInfo& batch = batches[iBatch];
+		batch.numConstraints += con.numConstraintRows;
+	}
+
+	for (int iPhase = 0; iPhase < numPhases; ++iPhase)
+	{
+		// if phase is legit,
+		if (iPhase == (iPhase & phaseMask))
+		{
+			int iBeginBatch = iPhase * maxNumBatchesPerPhase;
+			int iEndBatch = iBeginBatch + maxNumBatchesPerPhase;
+			mergeSmallBatches(batches, iBeginBatch, iEndBatch, minBatchSize, maxBatchSize);
+		}
+	}
+	// all constraints have been assigned a batchId
+	updateConstraintBatchIdsForMergesMt(constraintBatchIds, numConstraints, batches, maxNumBatchesPerPhase * numPhases);
+
+	if (numConstraintRows > numConstraints)
+	{
+		expandConstraintRowsMt(&constraintRowBatchIds[0], &constraintBatchIds[0], &conInfos[0], numConstraints, numConstraintRows);
+	}
+	else
+	{
+		constraintRowBatchIds = constraintBatchIds;
+	}
+
+	writeOutBatches(batchedConstraints, constraintRowBatchIds, numConstraintRows, batches, batchWork, maxNumBatchesPerPhase, numPhases);
+	btAssert(batchedConstraints->validate(constraints, bodies));
+}
+
+static void setupSingleBatch(
+	btBatchedConstraints* bc,
+	int numConstraints)
+{
+	BT_PROFILE("setupSingleBatch");
+	typedef btBatchedConstraints::Range Range;
+
+	bc->m_constraintIndices.resize(numConstraints);
+	for (int i = 0; i < numConstraints; ++i)
+	{
+		bc->m_constraintIndices[i] = i;
+	}
+
+	bc->m_batches.resizeNoInitialize(0);
+	bc->m_phases.resizeNoInitialize(0);
+	bc->m_phaseOrder.resizeNoInitialize(0);
+	bc->m_phaseGrainSize.resizeNoInitialize(0);
+
+	if (numConstraints > 0)
+	{
+		bc->m_batches.push_back(Range(0, numConstraints));
+		bc->m_phases.push_back(Range(0, 1));
+		bc->m_phaseOrder.push_back(0);
+		bc->m_phaseGrainSize.push_back(1);
+	}
+}
+
+void btBatchedConstraints::setup(
+	btConstraintArray* constraints,
+	const btAlignedObjectArray<btSolverBody>& bodies,
+	BatchingMethod batchingMethod,
+	int minBatchSize,
+	int maxBatchSize,
+	btAlignedObjectArray<char>* scratchMemory)
+{
+	if (constraints->size() >= minBatchSize * 4)
+	{
+		bool use2DGrid = batchingMethod == BATCHING_METHOD_SPATIAL_GRID_2D;
+		setupSpatialGridBatchesMt(this, scratchMemory, constraints, bodies, minBatchSize, maxBatchSize, use2DGrid);
+		if (s_debugDrawBatches)
+		{
+			debugDrawAllBatches(this, constraints, bodies);
+		}
+	}
+	else
+	{
+		setupSingleBatch(this, constraints->size());
+	}
+}
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.h
new file mode 100644
index 00000000000..5d982ca370c
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.h
@@ -0,0 +1,62 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_BATCHED_CONSTRAINTS_H
+#define BT_BATCHED_CONSTRAINTS_H
+
+#include "LinearMath/btThreads.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
+#include "BulletDynamics/ConstraintSolver/btSolverConstraint.h"
+
+class btIDebugDraw;
+
+struct btBatchedConstraints
+{
+	enum BatchingMethod
+	{
+		BATCHING_METHOD_SPATIAL_GRID_2D,
+		BATCHING_METHOD_SPATIAL_GRID_3D,
+		BATCHING_METHOD_COUNT
+	};
+	struct Range
+	{
+		int begin;
+		int end;
+
+		Range() : begin(0), end(0) {}
+		Range(int _beg, int _end) : begin(_beg), end(_end) {}
+	};
+
+	btAlignedObjectArray<int> m_constraintIndices;
+	btAlignedObjectArray<Range> m_batches;        // each batch is a range of indices in the m_constraintIndices array
+	btAlignedObjectArray<Range> m_phases;         // each phase is range of indices in the m_batches array
+	btAlignedObjectArray<char> m_phaseGrainSize;  // max grain size for each phase
+	btAlignedObjectArray<int> m_phaseOrder;       // phases can be done in any order, so we can randomize the order here
+	btIDebugDraw* m_debugDrawer;
+
+	static bool s_debugDrawBatches;
+
+	btBatchedConstraints() { m_debugDrawer = NULL; }
+	void setup(btConstraintArray* constraints,
+			   const btAlignedObjectArray<btSolverBody>& bodies,
+			   BatchingMethod batchingMethod,
+			   int minBatchSize,
+			   int maxBatchSize,
+			   btAlignedObjectArray<char>* scratchMemory);
+	bool validate(btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies) const;
+};
+
+#endif  // BT_BATCHED_CONSTRAINTS_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
index 09b7388b63e..ac046aa6eaf 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
@@ -15,49 +15,38 @@ subject to the following restrictions:
 Written by: Marcus Hennix
 */
 
-
 #include "btConeTwistConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
 #include "LinearMath/btMinMax.h"
+#include <cmath>
 #include <new>
 
-
-
 //#define CONETWIST_USE_OBSOLETE_SOLVER true
 #define CONETWIST_USE_OBSOLETE_SOLVER false
 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
 
-
 SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
 {
 	btVector3 vec = axis * invInertiaWorld;
 	return axis.dot(vec);
 }
 
-
-
-
-btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, 
-											 const btTransform& rbAFrame,const btTransform& rbBFrame)
-											 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
-											 m_angularOnly(false),
-											 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
+btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA, btRigidBody& rbB,
+											 const btTransform& rbAFrame, const btTransform& rbBFrame)
+	: btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA, rbB), m_rbAFrame(rbAFrame), m_rbBFrame(rbBFrame), m_angularOnly(false), m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
 {
 	init();
 }
 
-btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
-											:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
-											 m_angularOnly(false),
-											 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
+btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
+	: btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA), m_rbAFrame(rbAFrame), m_angularOnly(false), m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
 {
 	m_rbBFrame = m_rbAFrame;
 	m_rbBFrame.setOrigin(btVector3(0., 0., 0.));
-	init();	
+	init();
 }
 
-
 void btConeTwistConstraint::init()
 {
 	m_angularOnly = false;
@@ -75,30 +64,29 @@ void btConeTwistConstraint::init()
 	m_angCFM = btScalar(0.f);
 }
 
-
-void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
+void btConeTwistConstraint::getInfo1(btConstraintInfo1* info)
 {
 	if (m_useSolveConstraintObsolete)
 	{
 		info->m_numConstraintRows = 0;
 		info->nub = 0;
-	} 
+	}
 	else
 	{
 		info->m_numConstraintRows = 3;
 		info->nub = 3;
-		calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
-		if(m_solveSwingLimit)
+		calcAngleInfo2(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld());
+		if (m_solveSwingLimit)
 		{
 			info->m_numConstraintRows++;
 			info->nub--;
-			if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+			if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
 			{
 				info->m_numConstraintRows++;
 				info->nub--;
 			}
 		}
-		if(m_solveTwistLimit)
+		if (m_solveTwistLimit)
 		{
 			info->m_numConstraintRows++;
 			info->nub--;
@@ -106,90 +94,88 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
 	}
 }
 
-void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+void btConeTwistConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
 {
 	//always reserve 6 rows: object transform is not available on SPU
 	info->m_numConstraintRows = 6;
 	info->nub = 0;
-		
 }
-	
 
-void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
+void btConeTwistConstraint::getInfo2(btConstraintInfo2* info)
 {
-	getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
+	getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld());
 }
 
-void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
+void btConeTwistConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB)
 {
-	calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB);
-	
+	calcAngleInfo2(transA, transB, invInertiaWorldA, invInertiaWorldB);
+
 	btAssert(!m_useSolveConstraintObsolete);
-    // set jacobian
-    info->m_J1linearAxis[0] = 1;
-    info->m_J1linearAxis[info->rowskip+1] = 1;
-    info->m_J1linearAxis[2*info->rowskip+2] = 1;
+	// set jacobian
+	info->m_J1linearAxis[0] = 1;
+	info->m_J1linearAxis[info->rowskip + 1] = 1;
+	info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
 	btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin();
 	{
 		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
-		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * info->rowskip);
 		btVector3 a1neg = -a1;
-		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
-    info->m_J2linearAxis[0] = -1;
-    info->m_J2linearAxis[info->rowskip+1] = -1;
-    info->m_J2linearAxis[2*info->rowskip+2] = -1;
+	info->m_J2linearAxis[0] = -1;
+	info->m_J2linearAxis[info->rowskip + 1] = -1;
+	info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
 	btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin();
 	{
 		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
-		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
-		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
-    // set right hand side
+	// set right hand side
 	btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
-    btScalar k = info->fps * linERP;
-    int j;
-	for (j=0; j<3; j++)
-    {
-        info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
-		info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
-		info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
-		if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
+	btScalar k = info->fps * linERP;
+	int j;
+	for (j = 0; j < 3; j++)
+	{
+		info->m_constraintError[j * info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
+		info->m_lowerLimit[j * info->rowskip] = -SIMD_INFINITY;
+		info->m_upperLimit[j * info->rowskip] = SIMD_INFINITY;
+		if (m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
 		{
-			info->cfm[j*info->rowskip] = m_linCFM;
+			info->cfm[j * info->rowskip] = m_linCFM;
 		}
-    }
+	}
 	int row = 3;
-    int srow = row * info->rowskip;
+	int srow = row * info->rowskip;
 	btVector3 ax1;
 	// angular limits
-	if(m_solveSwingLimit)
+	if (m_solveSwingLimit)
 	{
-		btScalar *J1 = info->m_J1angularAxis;
-		btScalar *J2 = info->m_J2angularAxis;
-		if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+		btScalar* J1 = info->m_J1angularAxis;
+		btScalar* J2 = info->m_J2angularAxis;
+		if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
 		{
-			btTransform trA = transA*m_rbAFrame;
+			btTransform trA = transA * m_rbAFrame;
 			btVector3 p = trA.getBasis().getColumn(1);
 			btVector3 q = trA.getBasis().getColumn(2);
 			int srow1 = srow + info->rowskip;
-			J1[srow+0] = p[0];
-			J1[srow+1] = p[1];
-			J1[srow+2] = p[2];
-			J1[srow1+0] = q[0];
-			J1[srow1+1] = q[1];
-			J1[srow1+2] = q[2];
-			J2[srow+0] = -p[0];
-			J2[srow+1] = -p[1];
-			J2[srow+2] = -p[2];
-			J2[srow1+0] = -q[0];
-			J2[srow1+1] = -q[1];
-			J2[srow1+2] = -q[2];
+			J1[srow + 0] = p[0];
+			J1[srow + 1] = p[1];
+			J1[srow + 2] = p[2];
+			J1[srow1 + 0] = q[0];
+			J1[srow1 + 1] = q[1];
+			J1[srow1 + 2] = q[2];
+			J2[srow + 0] = -p[0];
+			J2[srow + 1] = -p[1];
+			J2[srow + 2] = -p[2];
+			J2[srow1 + 0] = -q[0];
+			J2[srow1 + 1] = -q[1];
+			J2[srow1 + 2] = -q[2];
 			btScalar fact = info->fps * m_relaxationFactor;
-			info->m_constraintError[srow] =   fact * m_swingAxis.dot(p);
-			info->m_constraintError[srow1] =  fact * m_swingAxis.dot(q);
+			info->m_constraintError[srow] = fact * m_swingAxis.dot(p);
+			info->m_constraintError[srow1] = fact * m_swingAxis.dot(q);
 			info->m_lowerLimit[srow] = -SIMD_INFINITY;
 			info->m_upperLimit[srow] = SIMD_INFINITY;
 			info->m_lowerLimit[srow1] = -SIMD_INFINITY;
@@ -199,16 +185,16 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
 		else
 		{
 			ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
-			J1[srow+0] = ax1[0];
-			J1[srow+1] = ax1[1];
-			J1[srow+2] = ax1[2];
-			J2[srow+0] = -ax1[0];
-			J2[srow+1] = -ax1[1];
-			J2[srow+2] = -ax1[2];
+			J1[srow + 0] = ax1[0];
+			J1[srow + 1] = ax1[1];
+			J1[srow + 2] = ax1[2];
+			J2[srow + 0] = -ax1[0];
+			J2[srow + 1] = -ax1[1];
+			J2[srow + 2] = -ax1[2];
 			btScalar k = info->fps * m_biasFactor;
 
 			info->m_constraintError[srow] = k * m_swingCorrection;
-			if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
+			if (m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
 			{
 				info->cfm[srow] = m_angCFM;
 			}
@@ -218,36 +204,35 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
 			srow += info->rowskip;
 		}
 	}
-	if(m_solveTwistLimit)
+	if (m_solveTwistLimit)
 	{
 		ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
-		btScalar *J1 = info->m_J1angularAxis;
-		btScalar *J2 = info->m_J2angularAxis;
-		J1[srow+0] = ax1[0];
-		J1[srow+1] = ax1[1];
-		J1[srow+2] = ax1[2];
-		J2[srow+0] = -ax1[0];
-		J2[srow+1] = -ax1[1];
-		J2[srow+2] = -ax1[2];
+		btScalar* J1 = info->m_J1angularAxis;
+		btScalar* J2 = info->m_J2angularAxis;
+		J1[srow + 0] = ax1[0];
+		J1[srow + 1] = ax1[1];
+		J1[srow + 2] = ax1[2];
+		J2[srow + 0] = -ax1[0];
+		J2[srow + 1] = -ax1[1];
+		J2[srow + 2] = -ax1[2];
 		btScalar k = info->fps * m_biasFactor;
 		info->m_constraintError[srow] = k * m_twistCorrection;
-		if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
+		if (m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
 		{
 			info->cfm[srow] = m_angCFM;
 		}
-		if(m_twistSpan > 0.0f)
+		if (m_twistSpan > 0.0f)
 		{
-
-			if(m_twistCorrection > 0.0f)
+			if (m_twistCorrection > 0.0f)
 			{
 				info->m_lowerLimit[srow] = 0;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
-			} 
+			}
 			else
 			{
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = 0;
-			} 
+			}
 		}
 		else
 		{
@@ -257,22 +242,20 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
 		srow += info->rowskip;
 	}
 }
-	
 
-
-void	btConeTwistConstraint::buildJacobian()
+void btConeTwistConstraint::buildJacobian()
 {
 	if (m_useSolveConstraintObsolete)
 	{
 		m_appliedImpulse = btScalar(0.);
 		m_accTwistLimitImpulse = btScalar(0.);
 		m_accSwingLimitImpulse = btScalar(0.);
-		m_accMotorImpulse = btVector3(0.,0.,0.);
+		m_accMotorImpulse = btVector3(0., 0., 0.);
 
 		if (!m_angularOnly)
 		{
-			btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-			btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+			btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
+			btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
 			btVector3 relPos = pivotBInW - pivotAInW;
 
 			btVector3 normal[3];
@@ -282,71 +265,68 @@ void	btConeTwistConstraint::buildJacobian()
 			}
 			else
 			{
-				normal[0].setValue(btScalar(1.0),0,0);
+				normal[0].setValue(btScalar(1.0), 0, 0);
 			}
 
 			btPlaneSpace1(normal[0], normal[1], normal[2]);
 
-			for (int i=0;i<3;i++)
+			for (int i = 0; i < 3; i++)
 			{
 				new (&m_jac[i]) btJacobianEntry(
-				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-				pivotAInW - m_rbA.getCenterOfMassPosition(),
-				pivotBInW - m_rbB.getCenterOfMassPosition(),
-				normal[i],
-				m_rbA.getInvInertiaDiagLocal(),
-				m_rbA.getInvMass(),
-				m_rbB.getInvInertiaDiagLocal(),
-				m_rbB.getInvMass());
+					m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+					m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+					pivotAInW - m_rbA.getCenterOfMassPosition(),
+					pivotBInW - m_rbB.getCenterOfMassPosition(),
+					normal[i],
+					m_rbA.getInvInertiaDiagLocal(),
+					m_rbA.getInvMass(),
+					m_rbB.getInvInertiaDiagLocal(),
+					m_rbB.getInvMass());
 			}
 		}
 
-		calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
+		calcAngleInfo2(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getInvInertiaTensorWorld(), m_rbB.getInvInertiaTensorWorld());
 	}
 }
 
-
-
-void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
+void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA, btSolverBody& bodyB, btScalar timeStep)
 {
-	#ifndef __SPU__
+#ifndef __SPU__
 	if (m_useSolveConstraintObsolete)
 	{
-		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
+		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
 
 		btScalar tau = btScalar(0.3);
 
 		//linear part
 		if (!m_angularOnly)
 		{
-			btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+			btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
 			btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
 
 			btVector3 vel1;
-			bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
+			bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1, vel1);
 			btVector3 vel2;
-			bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
+			bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2, vel2);
 			btVector3 vel = vel1 - vel2;
 
-			for (int i=0;i<3;i++)
-			{		
+			for (int i = 0; i < 3; i++)
+			{
 				const btVector3& normal = m_jac[i].m_linearJointAxis;
 				btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
 
 				btScalar rel_vel;
 				rel_vel = normal.dot(vel);
 				//positional error (zeroth order error)
-				btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
-				btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
+				btScalar depth = -(pivotAInW - pivotBInW).dot(normal);  //this is the error projected on the normal
+				btScalar impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
 				m_appliedImpulse += impulse;
-				
+
 				btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
 				btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
-				bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
-				bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
-		
+				bodyA.internalApplyImpulse(normal * m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld() * ftorqueAxis1, impulse);
+				bodyB.internalApplyImpulse(normal * m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld() * ftorqueAxis2, -impulse);
 			}
 		}
 
@@ -356,13 +336,17 @@ void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
 			// compute current and predicted transforms
 			btTransform trACur = m_rbA.getCenterOfMassTransform();
 			btTransform trBCur = m_rbB.getCenterOfMassTransform();
-			btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
-			btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
-			btTransform trAPred; trAPred.setIdentity(); 
-			btVector3 zerovec(0,0,0);
+			btVector3 omegaA;
+			bodyA.internalGetAngularVelocity(omegaA);
+			btVector3 omegaB;
+			bodyB.internalGetAngularVelocity(omegaB);
+			btTransform trAPred;
+			trAPred.setIdentity();
+			btVector3 zerovec(0, 0, 0);
 			btTransformUtil::integrateTransform(
 				trACur, zerovec, omegaA, timeStep, trAPred);
-			btTransform trBPred; trBPred.setIdentity(); 
+			btTransform trBPred;
+			trBPred.setIdentity();
 			btTransformUtil::integrateTransform(
 				trBCur, zerovec, omegaB, timeStep, trBPred);
 
@@ -374,7 +358,7 @@ void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
 
 			// compute desired omegas in world
 			btVector3 omegaADes, omegaBDes;
-			
+
 			btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
 			btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
 
@@ -415,10 +399,10 @@ void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
 				{
 					btScalar fMaxImpulse = m_maxMotorImpulse;
 					if (m_bNormalizedMotorStrength)
-						fMaxImpulse = fMaxImpulse/kAxisAInv;
+						fMaxImpulse = fMaxImpulse / kAxisAInv;
 
 					btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
-					btScalar  newUnclampedMag = newUnclampedAccImpulse.length();
+					btScalar newUnclampedMag = newUnclampedAccImpulse.length();
 					if (newUnclampedMag > fMaxImpulse)
 					{
 						newUnclampedAccImpulse.normalize();
@@ -428,31 +412,32 @@ void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
 					m_accMotorImpulse += impulse;
 				}
 
-				btScalar  impulseMag  = impulse.length();
-				btVector3 impulseAxis =  impulse / impulseMag;
-
-				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
-				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+				btScalar impulseMag = impulse.length();
+				btVector3 impulseAxis = impulse / impulseMag;
 
+				bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * impulseAxis, impulseMag);
+				bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * impulseAxis, -impulseMag);
 			}
 		}
-		else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
+		else if (m_damping > SIMD_EPSILON)  // no motor: do a little damping
 		{
-			btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
-			btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
+			btVector3 angVelA;
+			bodyA.internalGetAngularVelocity(angVelA);
+			btVector3 angVelB;
+			bodyB.internalGetAngularVelocity(angVelB);
 			btVector3 relVel = angVelB - angVelA;
 			if (relVel.length2() > SIMD_EPSILON)
 			{
 				btVector3 relVelAxis = relVel.normalized();
-				btScalar m_kDamping =  btScalar(1.) /
-					(getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
-					 getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
+				btScalar m_kDamping = btScalar(1.) /
+									  (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
+									   getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
 				btVector3 impulse = m_damping * m_kDamping * relVel;
 
-				btScalar  impulseMag  = impulse.length();
+				btScalar impulseMag = impulse.length();
 				btVector3 impulseAxis = impulse / impulseMag;
-				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
-				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+				bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * impulseAxis, impulseMag);
+				bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * impulseAxis, -impulseMag);
 			}
 		}
 
@@ -467,7 +452,7 @@ void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
 			// solve swing limit
 			if (m_solveSwingLimit)
 			{
-				btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
+				btScalar amplitude = m_swingLimitRatio * m_swingCorrection * m_biasFactor / timeStep;
 				btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
 				if (relSwingVel > 0)
 					amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
@@ -475,7 +460,7 @@ void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
 
 				// Clamp the accumulated impulse
 				btScalar temp = m_accSwingLimitImpulse;
-				m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
+				m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0));
 				impulseMag = m_accSwingLimitImpulse - temp;
 
 				btVector3 impulse = m_swingAxis * impulseMag;
@@ -491,47 +476,41 @@ void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
 				impulseMag = impulse.length();
 				btVector3 noTwistSwingAxis = impulse / impulseMag;
 
-				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
-				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
+				bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * noTwistSwingAxis, impulseMag);
+				bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * noTwistSwingAxis, -impulseMag);
 			}
 
-
 			// solve twist limit
 			if (m_solveTwistLimit)
 			{
-				btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
-				btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis );
-				if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
+				btScalar amplitude = m_twistLimitRatio * m_twistCorrection * m_biasFactor / timeStep;
+				btScalar relTwistVel = (angVelB - angVelA).dot(m_twistAxis);
+				if (relTwistVel > 0)  // only damp when moving towards limit (m_twistAxis flipping is important)
 					amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
 				btScalar impulseMag = amplitude * m_kTwist;
 
 				// Clamp the accumulated impulse
 				btScalar temp = m_accTwistLimitImpulse;
-				m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
+				m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0));
 				impulseMag = m_accTwistLimitImpulse - temp;
 
-		//		btVector3 impulse = m_twistAxis * impulseMag;
+				//		btVector3 impulse = m_twistAxis * impulseMag;
 
-				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
-				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
-			}		
+				bodyA.internalApplyImpulse(btVector3(0, 0, 0), m_rbA.getInvInertiaTensorWorld() * m_twistAxis, impulseMag);
+				bodyB.internalApplyImpulse(btVector3(0, 0, 0), m_rbB.getInvInertiaTensorWorld() * m_twistAxis, -impulseMag);
+			}
 		}
 	}
 #else
-btAssert(0);
-#endif //__SPU__
+	btAssert(0);
+#endif  //__SPU__
 }
 
-
-
-
-void	btConeTwistConstraint::updateRHS(btScalar	timeStep)
+void btConeTwistConstraint::updateRHS(btScalar timeStep)
 {
 	(void)timeStep;
-
 }
 
-
 #ifndef __SPU__
 void btConeTwistConstraint::calcAngleInfo()
 {
@@ -540,15 +519,15 @@ void btConeTwistConstraint::calcAngleInfo()
 	m_solveTwistLimit = false;
 	m_solveSwingLimit = false;
 
-	btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0);
-	btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0);
+	btVector3 b1Axis1(0, 0, 0), b1Axis2(0, 0, 0), b1Axis3(0, 0, 0);
+	btVector3 b2Axis1(0, 0, 0), b2Axis2(0, 0, 0);
 
 	b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
 	b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
 
-	btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
+	btScalar swing1 = btScalar(0.), swing2 = btScalar(0.);
 
-	btScalar swx=btScalar(0.),swy = btScalar(0.);
+	btScalar swx = btScalar(0.), swy = btScalar(0.);
 	btScalar thresh = btScalar(10.);
 	btScalar fact;
 
@@ -558,33 +537,33 @@ void btConeTwistConstraint::calcAngleInfo()
 		b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
 		swx = b2Axis1.dot(b1Axis1);
 		swy = b2Axis1.dot(b1Axis2);
-		swing1  = btAtan2Fast(swy, swx);
-		fact = (swy*swy + swx*swx) * thresh * thresh;
+		swing1 = btAtan2Fast(swy, swx);
+		fact = (swy * swy + swx * swx) * thresh * thresh;
 		fact = fact / (fact + btScalar(1.0));
-		swing1 *= fact; 
+		swing1 *= fact;
 	}
 
 	if (m_swingSpan2 >= btScalar(0.05f))
 	{
-		b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);			
+		b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
 		swx = b2Axis1.dot(b1Axis1);
 		swy = b2Axis1.dot(b1Axis3);
-		swing2  = btAtan2Fast(swy, swx);
-		fact = (swy*swy + swx*swx) * thresh * thresh;
+		swing2 = btAtan2Fast(swy, swx);
+		fact = (swy * swy + swx * swx) * thresh * thresh;
 		fact = fact / (fact + btScalar(1.0));
-		swing2 *= fact; 
+		swing2 *= fact;
 	}
 
-	btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);		
-	btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);	
-	btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
+	btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1);
+	btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2);
+	btScalar EllipseAngle = btFabs(swing1 * swing1) * RMaxAngle1Sq + btFabs(swing2 * swing2) * RMaxAngle2Sq;
 
 	if (EllipseAngle > 1.0f)
 	{
-		m_swingCorrection = EllipseAngle-1.0f;
+		m_swingCorrection = EllipseAngle - 1.0f;
 		m_solveSwingLimit = true;
 		// Calculate necessary axis & factors
-		m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
+		m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3));
 		m_swingAxis.normalize();
 		btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
 		m_swingAxis *= swingAxisSign;
@@ -594,14 +573,14 @@ void btConeTwistConstraint::calcAngleInfo()
 	if (m_twistSpan >= btScalar(0.))
 	{
 		btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
-		btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
-		btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 
-		btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
+		btQuaternion rotationArc = shortestArcQuat(b2Axis1, b1Axis1);
+		btVector3 TwistRef = quatRotate(rotationArc, b2Axis2);
+		btScalar twist = btAtan2Fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));
 		m_twistAngle = twist;
 
-//		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
+		//		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
 		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
-		if (twist <= -m_twistSpan*lockedFreeFactor)
+		if (twist <= -m_twistSpan * lockedFreeFactor)
 		{
 			m_twistCorrection = -(twist + m_twistSpan);
 			m_solveTwistLimit = true;
@@ -609,7 +588,7 @@ void btConeTwistConstraint::calcAngleInfo()
 			m_twistAxis.normalize();
 			m_twistAxis *= -1.0f;
 		}
-		else if (twist >  m_twistSpan*lockedFreeFactor)
+		else if (twist > m_twistSpan * lockedFreeFactor)
 		{
 			m_twistCorrection = (twist - m_twistSpan);
 			m_solveTwistLimit = true;
@@ -618,13 +597,11 @@ void btConeTwistConstraint::calcAngleInfo()
 		}
 	}
 }
-#endif //__SPU__
-
-static btVector3 vTwist(1,0,0); // twist axis in constraint's space
+#endif  //__SPU__
 
+static btVector3 vTwist(1, 0, 0);  // twist axis in constraint's space
 
-
-void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
+void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB)
 {
 	m_swingCorrection = btScalar(0.);
 	m_twistLimitSign = btScalar(0.);
@@ -632,7 +609,7 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 	m_solveSwingLimit = false;
 	// compute rotation of A wrt B (in constraint space)
 	if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
-	{	// it is assumed that setMotorTarget() was alredy called 
+	{  // it is assumed that setMotorTarget() was alredy called
 		// and motor target m_qTarget is within constraint limits
 		// TODO : split rotation to pure swing and pure twist
 		// compute desired transforms in world
@@ -641,23 +618,22 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 		btTransform trB = transB * m_rbBFrame;
 		btTransform trDeltaAB = trB * trPose * trA.inverse();
 		btQuaternion qDeltaAB = trDeltaAB.getRotation();
-		btVector3 swingAxis = 	btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
-		float swingAxisLen2 = swingAxis.length2();
-		if(btFuzzyZero(swingAxisLen2))
+		btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
+		btScalar swingAxisLen2 = swingAxis.length2();
+		if (btFuzzyZero(swingAxisLen2))
 		{
-		   return;
+			return;
 		}
 		m_swingAxis = swingAxis;
 		m_swingAxis.normalize();
 		m_swingCorrection = qDeltaAB.getAngle();
-		if(!btFuzzyZero(m_swingCorrection))
+		if (!btFuzzyZero(m_swingCorrection))
 		{
 			m_solveSwingLimit = true;
 		}
 		return;
 	}
 
-
 	{
 		// compute rotation of A wrt B (in constraint space)
 		btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
@@ -665,13 +641,17 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 		btQuaternion qAB = qB.inverse() * qA;
 		// split rotation into cone and twist
 		// (all this is done from B's perspective. Maybe I should be averaging axes...)
-		btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
-		btQuaternion qABCone  = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
-		btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
+		btVector3 vConeNoTwist = quatRotate(qAB, vTwist);
+		vConeNoTwist.normalize();
+		btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist);
+		qABCone.normalize();
+		btQuaternion qABTwist = qABCone.inverse() * qAB;
+		qABTwist.normalize();
 
 		if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
 		{
-			btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
+			btScalar swingAngle, swingLimit = 0;
+			btVector3 swingAxis;
 			computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
 
 			if (swingAngle > swingLimit * m_limitSoftness)
@@ -684,9 +664,9 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 				m_swingLimitRatio = 1.f;
 				if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
 				{
-					m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
+					m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness) /
 										(swingLimit - swingLimit * m_limitSoftness);
-				}				
+				}
 
 				// swing correction tries to get back to soft limit
 				m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
@@ -694,14 +674,14 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 				// adjustment of swing axis (based on ellipse normal)
 				adjustSwingAxisToUseEllipseNormal(swingAxis);
 
-				// Calculate necessary axis & factors		
+				// Calculate necessary axis & factors
 				m_swingAxis = quatRotate(qB, -swingAxis);
 
-				m_twistAxisA.setValue(0,0,0);
+				m_twistAxisA.setValue(0, 0, 0);
 
-				m_kSwing =  btScalar(1.) /
-					(computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
-					 computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
+				m_kSwing = btScalar(1.) /
+						   (computeAngularImpulseDenominator(m_swingAxis, invInertiaWorldA) +
+							computeAngularImpulseDenominator(m_swingAxis, invInertiaWorldB));
 			}
 		}
 		else
@@ -717,9 +697,9 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 			btScalar x = ivB.dot(ivA);
 			btScalar y = ivB.dot(jvA);
 			btScalar z = ivB.dot(kvA);
-			if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
-			{ // fixed. We'll need to add one more row to constraint
-				if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
+			if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+			{  // fixed. We'll need to add one more row to constraint
+				if ((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
 				{
 					m_solveSwingLimit = true;
 					m_swingAxis = -ivB.cross(ivA);
@@ -727,47 +707,47 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 			}
 			else
 			{
-				if(m_swingSpan1 < m_fixThresh)
-				{ // hinge around Y axis
-//					if(!(btFuzzyZero(y)))
-					if((!(btFuzzyZero(x))) || (!(btFuzzyZero(z))))
+				if (m_swingSpan1 < m_fixThresh)
+				{  // hinge around Y axis
+					//					if(!(btFuzzyZero(y)))
+					if ((!(btFuzzyZero(x))) || (!(btFuzzyZero(z))))
 					{
 						m_solveSwingLimit = true;
-						if(m_swingSpan2 >= m_fixThresh)
+						if (m_swingSpan2 >= m_fixThresh)
 						{
 							y = btScalar(0.f);
 							btScalar span2 = btAtan2(z, x);
-							if(span2 > m_swingSpan2)
+							if (span2 > m_swingSpan2)
 							{
 								x = btCos(m_swingSpan2);
 								z = btSin(m_swingSpan2);
 							}
-							else if(span2 < -m_swingSpan2)
+							else if (span2 < -m_swingSpan2)
 							{
-								x =  btCos(m_swingSpan2);
+								x = btCos(m_swingSpan2);
 								z = -btSin(m_swingSpan2);
 							}
 						}
 					}
 				}
 				else
-				{ // hinge around Z axis
-//					if(!btFuzzyZero(z))
-					if((!(btFuzzyZero(x))) || (!(btFuzzyZero(y))))
+				{  // hinge around Z axis
+					//					if(!btFuzzyZero(z))
+					if ((!(btFuzzyZero(x))) || (!(btFuzzyZero(y))))
 					{
 						m_solveSwingLimit = true;
-						if(m_swingSpan1 >= m_fixThresh)
+						if (m_swingSpan1 >= m_fixThresh)
 						{
 							z = btScalar(0.f);
 							btScalar span1 = btAtan2(y, x);
-							if(span1 > m_swingSpan1)
+							if (span1 > m_swingSpan1)
 							{
 								x = btCos(m_swingSpan1);
 								y = btSin(m_swingSpan1);
 							}
-							else if(span1 < -m_swingSpan1)
+							else if (span1 < -m_swingSpan1)
 							{
-								x =  btCos(m_swingSpan1);
+								x = btCos(m_swingSpan1);
 								y = -btSin(m_swingSpan1);
 							}
 						}
@@ -778,10 +758,10 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 				target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
 				target.normalize();
 				m_swingAxis = -ivB.cross(target);
-                                m_swingCorrection = m_swingAxis.length();
+				m_swingCorrection = m_swingAxis.length();
 
-                                if (!btFuzzyZero(m_swingCorrection))
-                                    m_swingAxis.normalize();
+				if (!btFuzzyZero(m_swingCorrection))
+					m_swingAxis.normalize();
 			}
 		}
 
@@ -790,15 +770,15 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 			btVector3 twistAxis;
 			computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
 
-			if (m_twistAngle > m_twistSpan*m_limitSoftness)
+			if (m_twistAngle > m_twistSpan * m_limitSoftness)
 			{
 				m_solveTwistLimit = true;
 
 				m_twistLimitRatio = 1.f;
 				if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
 				{
-					m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
-										(m_twistSpan  - m_twistSpan * m_limitSoftness);
+					m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness) /
+										(m_twistSpan - m_twistSpan * m_limitSoftness);
 				}
 
 				// twist correction tries to get back to soft limit
@@ -807,8 +787,8 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 				m_twistAxis = quatRotate(qB, -twistAxis);
 
 				m_kTwist = btScalar(1.) /
-					(computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
-					 computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
+						   (computeAngularImpulseDenominator(m_twistAxis, invInertiaWorldA) +
+							computeAngularImpulseDenominator(m_twistAxis, invInertiaWorldB));
 			}
 
 			if (m_solveSwingLimit)
@@ -821,15 +801,13 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
 	}
 }
 
-
-
 // given a cone rotation in constraint space, (pre: twist must already be removed)
 // this method computes its corresponding swing angle and axis.
 // more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
 void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
-												 btScalar& swingAngle, // out
-												 btVector3& vSwingAxis, // out
-												 btScalar& swingLimit) // out
+												 btScalar& swingAngle,   // out
+												 btVector3& vSwingAxis,  // out
+												 btScalar& swingLimit)   // out
 {
 	swingAngle = qCone.getAngle();
 	if (swingAngle > SIMD_EPSILON)
@@ -840,7 +818,7 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
         // non-zero twist?! this should never happen.
        btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
 #endif
-        
+
 		// Compute limit for given swing. tricky:
 		// Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
 		// (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
@@ -848,7 +826,7 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
 		// For starters, compute the direction from center to surface of ellipse.
 		// This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
 		// (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
-		btScalar xEllipse =  vSwingAxis.y();
+		btScalar xEllipse = vSwingAxis.y();
 		btScalar yEllipse = -vSwingAxis.z();
 
 		// Now, we use the slope of the vector (using x/yEllipse) and find the length
@@ -858,14 +836,14 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
 		//  a^2   b^2
 		// Do the math and it should be clear.
 
-		swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
+		swingLimit = m_swingSpan1;  // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
 		if (fabs(xEllipse) > SIMD_EPSILON)
 		{
-			btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
+			btScalar surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse);
 			btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
 			norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
 			btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
-			swingLimit = sqrt(swingLimit2);
+			swingLimit = std::sqrt(swingLimit2);
 		}
 
 		// test!
@@ -887,7 +865,7 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
 #if 0
         btAssert(0);
 #endif
- 	}
+	}
 }
 
 btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
@@ -903,34 +881,34 @@ btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btSc
 	//  a^2   b^2
 	// Do the math and it should be clear.
 
-	float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
+	btScalar swingLimit = m_swingSpan1;  // if xEllipse == 0, just use axis b (1)
 	if (fabs(xEllipse) > SIMD_EPSILON)
 	{
-		btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
+		btScalar surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse);
 		btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
 		norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
 		btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
-		swingLimit = sqrt(swingLimit2);
+		swingLimit = std::sqrt(swingLimit2);
 	}
 
 	// convert into point in constraint space:
 	// note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
 	btVector3 vSwingAxis(0, xEllipse, -yEllipse);
 	btQuaternion qSwing(vSwingAxis, swingLimit);
-	btVector3 vPointInConstraintSpace(fLength,0,0);
+	btVector3 vPointInConstraintSpace(fLength, 0, 0);
 	return quatRotate(qSwing, vPointInConstraintSpace);
 }
 
 // given a twist rotation in constraint space, (pre: cone must already be removed)
 // this method computes its corresponding angle and axis.
 void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
-												  btScalar& twistAngle, // out
-												  btVector3& vTwistAxis) // out
+												  btScalar& twistAngle,   // out
+												  btVector3& vTwistAxis)  // out
 {
 	btQuaternion qMinTwist = qTwist;
 	twistAngle = qTwist.getAngle();
 
-	if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
+	if (twistAngle > SIMD_PI)  // long way around. flip quat and recalculate.
 	{
 		qMinTwist = -(qTwist);
 		twistAngle = qMinTwist.getAngle();
@@ -948,80 +926,79 @@ void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
 		vTwistAxis.normalize();
 }
 
-
 void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
 {
 	// the swing axis is computed as the "twist-free" cone rotation,
 	// but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
-	// so, if we're outside the limits, the closest way back inside the cone isn't 
+	// so, if we're outside the limits, the closest way back inside the cone isn't
 	// along the vector back to the center. better (and more stable) to use the ellipse normal.
 
 	// convert swing axis to direction from center to surface of ellipse
 	// (ie. rotate 2D vector by PI/2)
 	btScalar y = -vSwingAxis.z();
-	btScalar z =  vSwingAxis.y();
+	btScalar z = vSwingAxis.y();
 
 	// do the math...
-	if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
+	if (fabs(z) > SIMD_EPSILON)  // avoid division by 0. and we don't need an update if z == 0.
 	{
 		// compute gradient/normal of ellipse surface at current "point"
-		btScalar grad = y/z;
+		btScalar grad = y / z;
 		grad *= m_swingSpan2 / m_swingSpan1;
 
 		// adjust y/z to represent normal at point (instead of vector to point)
 		if (y > 0)
-			y =  fabs(grad * z);
+			y = fabs(grad * z);
 		else
 			y = -fabs(grad * z);
 
 		// convert ellipse direction back to swing axis
 		vSwingAxis.setZ(-y);
-		vSwingAxis.setY( z);
+		vSwingAxis.setY(z);
 		vSwingAxis.normalize();
 	}
 }
 
-
-
-void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
+void btConeTwistConstraint::setMotorTarget(const btQuaternion& q)
 {
 	//btTransform trACur = m_rbA.getCenterOfMassTransform();
 	//btTransform trBCur = m_rbB.getCenterOfMassTransform();
-//	btTransform trABCur = trBCur.inverse() * trACur;
-//	btQuaternion qABCur = trABCur.getRotation();
-//	btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
+	//	btTransform trABCur = trBCur.inverse() * trACur;
+	//	btQuaternion qABCur = trABCur.getRotation();
+	//	btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
 	//btQuaternion qConstraintCur = trConstraintCur.getRotation();
 
 	btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
 	setMotorTargetInConstraintSpace(qConstraint);
 }
 
-
-void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
+void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion& q)
 {
 	m_qTarget = q;
 
 	// clamp motor target to within limits
 	{
-		btScalar softness = 1.f;//m_limitSoftness;
+		btScalar softness = 1.f;  //m_limitSoftness;
 
 		// split into twist and cone
 		btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
-		btQuaternion qTargetCone  = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
-		btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();
+		btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted);
+		qTargetCone.normalize();
+		btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget;
+		qTargetTwist.normalize();
 
 		// clamp cone
 		if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
 		{
-			btScalar swingAngle, swingLimit; btVector3 swingAxis;
+			btScalar swingAngle, swingLimit;
+			btVector3 swingAxis;
 			computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
 
 			if (fabs(swingAngle) > SIMD_EPSILON)
 			{
-				if (swingAngle > swingLimit*softness)
-					swingAngle = swingLimit*softness;
-				else if (swingAngle < -swingLimit*softness)
-					swingAngle = -swingLimit*softness;
+				if (swingAngle > swingLimit * softness)
+					swingAngle = swingLimit * softness;
+				else if (swingAngle < -swingLimit * softness)
+					swingAngle = -swingLimit * softness;
 				qTargetCone = btQuaternion(swingAxis, swingAngle);
 			}
 		}
@@ -1029,16 +1006,17 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &
 		// clamp twist
 		if (m_twistSpan >= btScalar(0.05f))
 		{
-			btScalar twistAngle; btVector3 twistAxis;
+			btScalar twistAngle;
+			btVector3 twistAxis;
 			computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
 
 			if (fabs(twistAngle) > SIMD_EPSILON)
 			{
 				// eddy todo: limitSoftness used here???
-				if (twistAngle > m_twistSpan*softness)
-					twistAngle = m_twistSpan*softness;
-				else if (twistAngle < -m_twistSpan*softness)
-					twistAngle = -m_twistSpan*softness;
+				if (twistAngle > m_twistSpan * softness)
+					twistAngle = m_twistSpan * softness;
+				else if (twistAngle < -m_twistSpan * softness)
+					twistAngle = -m_twistSpan * softness;
 				qTargetTwist = btQuaternion(twistAxis, twistAngle);
 			}
 		}
@@ -1047,15 +1025,15 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &
 	}
 }
 
-///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 ///If no axis is provided, it uses the default axis for this constraint.
 void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
 {
-	switch(num)
+	switch (num)
 	{
-		case BT_CONSTRAINT_ERP :
-		case BT_CONSTRAINT_STOP_ERP :
-			if((axis >= 0) && (axis < 3)) 
+		case BT_CONSTRAINT_ERP:
+		case BT_CONSTRAINT_STOP_ERP:
+			if ((axis >= 0) && (axis < 3))
 			{
 				m_linERP = value;
 				m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
@@ -1065,9 +1043,9 @@ void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
 				m_biasFactor = value;
 			}
 			break;
-		case BT_CONSTRAINT_CFM :
-		case BT_CONSTRAINT_STOP_CFM :
-			if((axis >= 0) && (axis < 3)) 
+		case BT_CONSTRAINT_CFM:
+		case BT_CONSTRAINT_STOP_CFM:
+			if ((axis >= 0) && (axis < 3))
 			{
 				m_linCFM = value;
 				m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
@@ -1085,19 +1063,19 @@ void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
 }
 
 ///return the local value of parameter
-btScalar btConeTwistConstraint::getParam(int num, int axis) const 
+btScalar btConeTwistConstraint::getParam(int num, int axis) const
 {
 	btScalar retVal = 0;
-	switch(num)
+	switch (num)
 	{
-		case BT_CONSTRAINT_ERP :
-		case BT_CONSTRAINT_STOP_ERP :
-			if((axis >= 0) && (axis < 3)) 
+		case BT_CONSTRAINT_ERP:
+		case BT_CONSTRAINT_STOP_ERP:
+			if ((axis >= 0) && (axis < 3))
 			{
 				btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
 				retVal = m_linERP;
 			}
-			else if((axis >= 3) && (axis < 6)) 
+			else if ((axis >= 3) && (axis < 6))
 			{
 				retVal = m_biasFactor;
 			}
@@ -1106,14 +1084,14 @@ btScalar btConeTwistConstraint::getParam(int num, int axis) const
 				btAssertConstrParams(0);
 			}
 			break;
-		case BT_CONSTRAINT_CFM :
-		case BT_CONSTRAINT_STOP_CFM :
-			if((axis >= 0) && (axis < 3)) 
+		case BT_CONSTRAINT_CFM:
+		case BT_CONSTRAINT_STOP_CFM:
+			if ((axis >= 0) && (axis < 3))
 			{
 				btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
 				retVal = m_linCFM;
 			}
-			else if((axis >= 3) && (axis < 6)) 
+			else if ((axis >= 3) && (axis < 6))
 			{
 				btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
 				retVal = m_angCFM;
@@ -1123,21 +1101,16 @@ btScalar btConeTwistConstraint::getParam(int num, int axis) const
 				btAssertConstrParams(0);
 			}
 			break;
-		default : 
+		default:
 			btAssertConstrParams(0);
 	}
 	return retVal;
 }
 
-
-void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
+void btConeTwistConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
 {
 	m_rbAFrame = frameA;
 	m_rbBFrame = frameB;
 	buildJacobian();
 	//calculateTransforms();
 }
-
- 
-
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
index b7636180c34..64f44df1cba 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
@@ -15,8 +15,6 @@ subject to the following restrictions:
 Written by: Marcus Hennix
 */
 
-
-
 /*
 Overview:
 
@@ -31,8 +29,6 @@ twist is along the x-axis,
 and swing 1 and 2 are along the z and y axes respectively.
 */
 
-
-
 #ifndef BT_CONETWISTCONSTRAINT_H
 #define BT_CONETWISTCONSTRAINT_H
 
@@ -41,13 +37,12 @@ and swing 1 and 2 are along the z and y axes respectively.
 #include "btTypedConstraint.h"
 
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btConeTwistConstraintData2	btConeTwistConstraintDoubleData
-#define btConeTwistConstraintDataName	"btConeTwistConstraintDoubleData"
+#define btConeTwistConstraintData2 btConeTwistConstraintDoubleData
+#define btConeTwistConstraintDataName "btConeTwistConstraintDoubleData"
 #else
-#define btConeTwistConstraintData2	btConeTwistConstraintData 
-#define btConeTwistConstraintDataName	"btConeTwistConstraintData" 
-#endif //BT_USE_DOUBLE_PRECISION
-
+#define btConeTwistConstraintData2 btConeTwistConstraintData
+#define btConeTwistConstraintDataName "btConeTwistConstraintData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
 class btRigidBody;
 
@@ -59,103 +54,99 @@ enum btConeTwistFlags
 };
 
 ///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
-ATTRIBUTE_ALIGNED16(class) btConeTwistConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class)
+btConeTwistConstraint : public btTypedConstraint
 {
 #ifdef IN_PARALLELL_SOLVER
 public:
 #endif
-	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
+	btJacobianEntry m_jac[3];  //3 orthogonal linear constraints
 
-	btTransform m_rbAFrame; 
+	btTransform m_rbAFrame;
 	btTransform m_rbBFrame;
 
-	btScalar	m_limitSoftness;
-	btScalar	m_biasFactor;
-	btScalar	m_relaxationFactor;
+	btScalar m_limitSoftness;
+	btScalar m_biasFactor;
+	btScalar m_relaxationFactor;
 
-	btScalar	m_damping;
+	btScalar m_damping;
 
-	btScalar	m_swingSpan1;
-	btScalar	m_swingSpan2;
-	btScalar	m_twistSpan;
+	btScalar m_swingSpan1;
+	btScalar m_swingSpan2;
+	btScalar m_twistSpan;
 
-	btScalar	m_fixThresh;
+	btScalar m_fixThresh;
 
-	btVector3   m_swingAxis;
-	btVector3	m_twistAxis;
+	btVector3 m_swingAxis;
+	btVector3 m_twistAxis;
 
-	btScalar	m_kSwing;
-	btScalar	m_kTwist;
+	btScalar m_kSwing;
+	btScalar m_kTwist;
 
-	btScalar	m_twistLimitSign;
-	btScalar	m_swingCorrection;
-	btScalar	m_twistCorrection;
+	btScalar m_twistLimitSign;
+	btScalar m_swingCorrection;
+	btScalar m_twistCorrection;
 
-	btScalar	m_twistAngle;
+	btScalar m_twistAngle;
 
-	btScalar	m_accSwingLimitImpulse;
-	btScalar	m_accTwistLimitImpulse;
+	btScalar m_accSwingLimitImpulse;
+	btScalar m_accTwistLimitImpulse;
 
-	bool		m_angularOnly;
-	bool		m_solveTwistLimit;
-	bool		m_solveSwingLimit;
+	bool m_angularOnly;
+	bool m_solveTwistLimit;
+	bool m_solveSwingLimit;
 
-	bool	m_useSolveConstraintObsolete;
+	bool m_useSolveConstraintObsolete;
 
 	// not yet used...
-	btScalar	m_swingLimitRatio;
-	btScalar	m_twistLimitRatio;
-	btVector3   m_twistAxisA;
+	btScalar m_swingLimitRatio;
+	btScalar m_twistLimitRatio;
+	btVector3 m_twistAxisA;
 
 	// motor
-	bool		 m_bMotorEnabled;
-	bool		 m_bNormalizedMotorStrength;
+	bool m_bMotorEnabled;
+	bool m_bNormalizedMotorStrength;
 	btQuaternion m_qTarget;
-	btScalar	 m_maxMotorImpulse;
-	btVector3	 m_accMotorImpulse;
-	
+	btScalar m_maxMotorImpulse;
+	btVector3 m_accMotorImpulse;
+
 	// parameters
-	int			m_flags;
-	btScalar	m_linCFM;
-	btScalar	m_linERP;
-	btScalar	m_angCFM;
-	
-protected:
+	int m_flags;
+	btScalar m_linCFM;
+	btScalar m_linERP;
+	btScalar m_angCFM;
 
+protected:
 	void init();
 
-	void computeConeLimitInfo(const btQuaternion& qCone, // in
-		btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
+	void computeConeLimitInfo(const btQuaternion& qCone,                                           // in
+							  btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit);  // all outs
 
-	void computeTwistLimitInfo(const btQuaternion& qTwist, // in
-		btScalar& twistAngle, btVector3& vTwistAxis); // all outs
-
-	void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
+	void computeTwistLimitInfo(const btQuaternion& qTwist,                    // in
+							   btScalar& twistAngle, btVector3& vTwistAxis);  // all outs
 
+	void adjustSwingAxisToUseEllipseNormal(btVector3 & vSwingAxis) const;
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame);
-	
-	btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
+	btConeTwistConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
+
+	btConeTwistConstraint(btRigidBody & rbA, const btTransform& rbAFrame);
+
+	virtual void buildJacobian();
 
-	virtual void	buildJacobian();
+	virtual void getInfo1(btConstraintInfo1 * info);
 
-	virtual void getInfo1 (btConstraintInfo1* info);
+	void getInfo1NonVirtual(btConstraintInfo1 * info);
 
-	void	getInfo1NonVirtual(btConstraintInfo1* info);
-	
-	virtual void getInfo2 (btConstraintInfo2* info);
-	
-	void	getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
+	virtual void getInfo2(btConstraintInfo2 * info);
 
-	virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
+	void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB);
 
-    
-	void	updateRHS(btScalar	timeStep);
+	virtual void solveConstraintObsolete(btSolverBody & bodyA, btSolverBody & bodyB, btScalar timeStep);
 
+	void updateRHS(btScalar timeStep);
 
 	const btRigidBody& getRigidBodyA() const
 	{
@@ -166,64 +157,64 @@ public:
 		return m_rbB;
 	}
 
-	void	setAngularOnly(bool angularOnly)
+	void setAngularOnly(bool angularOnly)
 	{
 		m_angularOnly = angularOnly;
 	}
-	
-	bool    getAngularOnly() const
+
+	bool getAngularOnly() const
 	{
-	    return m_angularOnly;
+		return m_angularOnly;
 	}
 
-	void	setLimit(int limitIndex,btScalar limitValue)
+	void setLimit(int limitIndex, btScalar limitValue)
 	{
 		switch (limitIndex)
 		{
-		case 3:
+			case 3:
 			{
 				m_twistSpan = limitValue;
 				break;
 			}
-		case 4:
+			case 4:
 			{
 				m_swingSpan2 = limitValue;
 				break;
 			}
-		case 5:
+			case 5:
 			{
 				m_swingSpan1 = limitValue;
 				break;
 			}
-		default:
+			default:
 			{
 			}
 		};
 	}
 
-    btScalar getLimit(int limitIndex) const
+	btScalar getLimit(int limitIndex) const
 	{
 		switch (limitIndex)
 		{
-		case 3:
+			case 3:
 			{
 				return m_twistSpan;
 				break;
 			}
-		case 4:
+			case 4:
 			{
 				return m_swingSpan2;
 				break;
 			}
-		case 5:
+			case 5:
 			{
 				return m_swingSpan1;
 				break;
 			}
-		default:
+			default:
 			{
-			    btAssert(0 && "Invalid limitIndex specified for btConeTwistConstraint");
-			    return 0.0;
+				btAssert(0 && "Invalid limitIndex specified for btConeTwistConstraint");
+				return 0.0;
 			}
 		};
 	}
@@ -239,18 +230,18 @@ public:
 	// __relaxationFactor:
 	//		0->1, recommend to stay near 1.
 	//		the lower the value, the less the constraint will fight velocities which violate the angular limits.
-	void	setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+	void setLimit(btScalar _swingSpan1, btScalar _swingSpan2, btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
 	{
 		m_swingSpan1 = _swingSpan1;
 		m_swingSpan2 = _swingSpan2;
-		m_twistSpan  = _twistSpan;
+		m_twistSpan = _twistSpan;
 
-		m_limitSoftness =  _softness;
+		m_limitSoftness = _softness;
 		m_biasFactor = _biasFactor;
 		m_relaxationFactor = _relaxationFactor;
 	}
 
-	const btTransform& getAFrame() const { return m_rbAFrame; };	
+	const btTransform& getAFrame() const { return m_rbAFrame; };
 	const btTransform& getBFrame() const { return m_rbBFrame; };
 
 	inline int getSolveTwistLimit()
@@ -260,7 +251,7 @@ public:
 
 	inline int getSolveSwingLimit()
 	{
-		return m_solveTwistLimit;
+		return m_solveSwingLimit;
 	}
 
 	inline btScalar getTwistLimitSign()
@@ -269,7 +260,7 @@ public:
 	}
 
 	void calcAngleInfo();
-	void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
+	void calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA, const btMatrix3x3& invInertiaWorldB);
 
 	inline btScalar getSwingSpan1() const
 	{
@@ -308,8 +299,16 @@ public:
 	bool isMotorEnabled() const { return m_bMotorEnabled; }
 	btScalar getMaxMotorImpulse() const { return m_maxMotorImpulse; }
 	bool isMaxMotorImpulseNormalized() const { return m_bNormalizedMotorStrength; }
-	void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
-	void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
+	void setMaxMotorImpulse(btScalar maxMotorImpulse)
+	{
+		m_maxMotorImpulse = maxMotorImpulse;
+		m_bNormalizedMotorStrength = false;
+	}
+	void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse)
+	{
+		m_maxMotorImpulse = maxMotorImpulse;
+		m_bNormalizedMotorStrength = true;
+	}
 
 	btScalar getFixThresh() { return m_fixThresh; }
 	void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
@@ -318,17 +317,17 @@ public:
 	// q: the desired rotation of bodyA wrt bodyB.
 	// note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
 	// note: don't forget to enableMotor()
-	void setMotorTarget(const btQuaternion &q);
+	void setMotorTarget(const btQuaternion& q);
 	const btQuaternion& getMotorTarget() const { return m_qTarget; }
 
 	// same as above, but q is the desired rotation of frameA wrt frameB in constraint space
-	void setMotorTargetInConstraintSpace(const btQuaternion &q);
+	void setMotorTargetInConstraintSpace(const btQuaternion& q);
 
 	btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 	///If no axis is provided, it uses the default axis for this constraint.
-	virtual	void setParam(int num, btScalar value, int axis = -1);
+	virtual void setParam(int num, btScalar value, int axis = -1);
 
 	virtual void setFrames(const btTransform& frameA, const btTransform& frameB);
 
@@ -342,84 +341,74 @@ public:
 		return m_rbBFrame;
 	}
 
-
 	///return the local value of parameter
-	virtual	btScalar getParam(int num, int axis = -1) const;
+	virtual btScalar getParam(int num, int axis = -1) const;
 
 	int getFlags() const
 	{
 		return m_flags;
 	}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	virtual int calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
-
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
-
-	
-struct	btConeTwistConstraintDoubleData
+struct btConeTwistConstraintDoubleData
 {
-	btTypedConstraintDoubleData	m_typeConstraintData;
+	btTypedConstraintDoubleData m_typeConstraintData;
 	btTransformDoubleData m_rbAFrame;
 	btTransformDoubleData m_rbBFrame;
 
 	//limits
-	double	m_swingSpan1;
-	double	m_swingSpan2;
-	double	m_twistSpan;
-	double	m_limitSoftness;
-	double	m_biasFactor;
-	double	m_relaxationFactor;
-
-	double	m_damping;
-		
-	
-
+	double m_swingSpan1;
+	double m_swingSpan2;
+	double m_twistSpan;
+	double m_limitSoftness;
+	double m_biasFactor;
+	double m_relaxationFactor;
+
+	double m_damping;
 };
 
 #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 ///this structure is not used, except for loading pre-2.82 .bullet files
-struct	btConeTwistConstraintData
+struct btConeTwistConstraintData
 {
-	btTypedConstraintData	m_typeConstraintData;
+	btTypedConstraintData m_typeConstraintData;
 	btTransformFloatData m_rbAFrame;
 	btTransformFloatData m_rbBFrame;
 
 	//limits
-	float	m_swingSpan1;
-	float	m_swingSpan2;
-	float	m_twistSpan;
-	float	m_limitSoftness;
-	float	m_biasFactor;
-	float	m_relaxationFactor;
-
-	float	m_damping;
-		
-	char m_pad[4];
+	float m_swingSpan1;
+	float m_swingSpan2;
+	float m_twistSpan;
+	float m_limitSoftness;
+	float m_biasFactor;
+	float m_relaxationFactor;
 
+	float m_damping;
+
+	char m_pad[4];
 };
-#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+#endif  //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 //
 
-SIMD_FORCE_INLINE int	btConeTwistConstraint::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(btConeTwistConstraintData2);
-
 }
 
-
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE const char*	btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*) dataBuffer;
-	btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
+	btConeTwistConstraintData2* cone = (btConeTwistConstraintData2*)dataBuffer;
+	btTypedConstraint::serialize(&cone->m_typeConstraintData, serializer);
 
 	m_rbAFrame.serialize(cone->m_rbAFrame);
 	m_rbBFrame.serialize(cone->m_rbBFrame);
-	
+
 	cone->m_swingSpan1 = m_swingSpan1;
 	cone->m_swingSpan2 = m_swingSpan2;
 	cone->m_twistSpan = m_twistSpan;
@@ -431,5 +420,4 @@ SIMD_FORCE_INLINE const char*	btConeTwistConstraint::serialize(void* dataBuffer,
 	return btConeTwistConstraintDataName;
 }
 
-
-#endif //BT_CONETWISTCONSTRAINT_H
+#endif  //BT_CONETWISTCONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
index 890afe6da42..68a4a07a1da 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
@@ -26,40 +26,34 @@ struct btContactSolverInfo;
 struct btBroadphaseProxy;
 class btIDebugDraw;
 class btStackAlloc;
-class	btDispatcher;
+class btDispatcher;
 /// btConstraintSolver provides solver interface
 
-
 enum btConstraintSolverType
 {
-	BT_SEQUENTIAL_IMPULSE_SOLVER=1,
-	BT_MLCP_SOLVER=2,
-	BT_NNCG_SOLVER=4
+	BT_SEQUENTIAL_IMPULSE_SOLVER = 1,
+	BT_MLCP_SOLVER = 2,
+	BT_NNCG_SOLVER = 4,
+	BT_MULTIBODY_SOLVER = 8,
+	BT_BLOCK_SOLVER = 16,
 };
 
 class btConstraintSolver
 {
-
 public:
-
 	virtual ~btConstraintSolver() {}
-	
-	virtual void prepareSolve (int /* numBodies */, int /* numManifolds */) {;}
+
+	virtual void prepareSolve(int /* numBodies */, int /* numManifolds */) { ; }
 
 	///solve a group of constraints
-	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer,btDispatcher* dispatcher) = 0;
+	virtual btScalar solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, class btIDebugDraw* debugDrawer, btDispatcher* dispatcher) = 0;
 
-	virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */) {;}
+	virtual void allSolved(const btContactSolverInfo& /* info */, class btIDebugDraw* /* debugDrawer */) { ; }
 
 	///clear internal cached data and reset random seed
-	virtual	void	reset() = 0;
-
-	virtual btConstraintSolverType	getSolverType() const=0;
-
+	virtual void reset() = 0;
 
+	virtual btConstraintSolverType getSolverType() const = 0;
 };
 
-
-
-
-#endif //BT_CONSTRAINT_SOLVER_H
+#endif  //BT_CONSTRAINT_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
index 1098d0c96b6..4b22b2fff5b 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btContactConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btVector3.h"
@@ -22,44 +21,33 @@ subject to the following restrictions:
 #include "LinearMath/btMinMax.h"
 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
 
-
-
-btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB)
-:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB),
-	m_contactManifold(*contactManifold)
+btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold, btRigidBody& rbA, btRigidBody& rbB)
+	: btTypedConstraint(CONTACT_CONSTRAINT_TYPE, rbA, rbB),
+	  m_contactManifold(*contactManifold)
 {
-
 }
 
 btContactConstraint::~btContactConstraint()
 {
-
 }
 
-void	btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
+void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
 {
 	m_contactManifold = *contactManifold;
 }
 
-void btContactConstraint::getInfo1 (btConstraintInfo1* info)
+void btContactConstraint::getInfo1(btConstraintInfo1* info)
 {
-
 }
 
-void btContactConstraint::getInfo2 (btConstraintInfo2* info)
+void btContactConstraint::getInfo2(btConstraintInfo2* info)
 {
-
 }
 
-void	btContactConstraint::buildJacobian()
+void btContactConstraint::buildJacobian()
 {
-
 }
 
-
-
-
-
 #include "btContactConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btVector3.h"
@@ -68,64 +56,59 @@ void	btContactConstraint::buildJacobian()
 #include "LinearMath/btMinMax.h"
 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
 
-
-
 //response  between two dynamic objects without friction and no restitution, assuming 0 penetration depth
 btScalar resolveSingleCollision(
-        btRigidBody* body1,
-        btCollisionObject* colObj2,
-		const btVector3& contactPositionWorld,
-		const btVector3& contactNormalOnB,
-        const btContactSolverInfo& solverInfo,
-		btScalar distance)
+	btRigidBody* body1,
+	btCollisionObject* colObj2,
+	const btVector3& contactPositionWorld,
+	const btVector3& contactNormalOnB,
+	const btContactSolverInfo& solverInfo,
+	btScalar distance)
 {
 	btRigidBody* body2 = btRigidBody::upcast(colObj2);
-    
-	
-    const btVector3& normal = contactNormalOnB;
-
-    btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin(); 
-    btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin();
-    
-    btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1);
-	btVector3 vel2 = body2? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
-    btVector3 vel = vel1 - vel2;
-    btScalar rel_vel;
-    rel_vel = normal.dot(vel);
-    
-    btScalar combinedRestitution = 0.f;
-    btScalar restitution = combinedRestitution* -rel_vel;
-
-    btScalar positionalError = solverInfo.m_erp *-distance /solverInfo.m_timeStep ;
-    btScalar velocityError = -(1.0f + restitution) * rel_vel;// * damping;
-	btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld,normal);
-	btScalar denom1 = body2? body2->computeImpulseDenominator(contactPositionWorld,normal) : 0.f;
+
+	const btVector3& normal = contactNormalOnB;
+
+	btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin();
+	btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin();
+
+	btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1);
+	btVector3 vel2 = body2 ? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0, 0, 0);
+	btVector3 vel = vel1 - vel2;
+	btScalar rel_vel;
+	rel_vel = normal.dot(vel);
+
+	btScalar combinedRestitution = 0.f;
+	btScalar restitution = combinedRestitution * -rel_vel;
+
+	btScalar positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep;
+	btScalar velocityError = -(1.0f + restitution) * rel_vel;  // * damping;
+	btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld, normal);
+	btScalar denom1 = body2 ? body2->computeImpulseDenominator(contactPositionWorld, normal) : 0.f;
 	btScalar relaxation = 1.f;
-	btScalar jacDiagABInv = relaxation/(denom0+denom1);
+	btScalar jacDiagABInv = relaxation / (denom0 + denom1);
 
-    btScalar penetrationImpulse = positionalError * jacDiagABInv;
-    btScalar velocityImpulse = velocityError * jacDiagABInv;
+	btScalar penetrationImpulse = positionalError * jacDiagABInv;
+	btScalar velocityImpulse = velocityError * jacDiagABInv;
 
-    btScalar normalImpulse = penetrationImpulse+velocityImpulse;
-    normalImpulse = 0.f > normalImpulse ? 0.f: normalImpulse;
+	btScalar normalImpulse = penetrationImpulse + velocityImpulse;
+	normalImpulse = 0.f > normalImpulse ? 0.f : normalImpulse;
 
-	body1->applyImpulse(normal*(normalImpulse), rel_pos1);
-    if (body2)
-		body2->applyImpulse(-normal*(normalImpulse), rel_pos2);
-    
-    return normalImpulse;
-}
+	body1->applyImpulse(normal * (normalImpulse), rel_pos1);
+	if (body2)
+		body2->applyImpulse(-normal * (normalImpulse), rel_pos2);
 
+	return normalImpulse;
+}
 
 //bilateral constraint between two dynamic objects
 void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
-                      btRigidBody& body2, const btVector3& pos2,
-                      btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
+							btRigidBody& body2, const btVector3& pos2,
+							btScalar distance, const btVector3& normal, btScalar& impulse, btScalar timeStep)
 {
 	(void)timeStep;
 	(void)distance;
 
-
 	btScalar normalLenSqr = normal.length2();
 	btAssert(btFabs(normalLenSqr) < btScalar(1.1));
 	if (normalLenSqr > btScalar(1.1))
@@ -133,45 +116,38 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
 		impulse = btScalar(0.);
 		return;
 	}
-	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
 	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
 	//this jacobian entry could be re-used for all iterations
-	
+
 	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
 	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
 	btVector3 vel = vel1 - vel2;
-	
 
-	   btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
-		body2.getCenterOfMassTransform().getBasis().transpose(),
-		rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
-		body2.getInvInertiaDiagLocal(),body2.getInvMass());
+	btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
+						body2.getCenterOfMassTransform().getBasis().transpose(),
+						rel_pos1, rel_pos2, normal, body1.getInvInertiaDiagLocal(), body1.getInvMass(),
+						body2.getInvInertiaDiagLocal(), body2.getInvMass());
 
 	btScalar jacDiagAB = jac.getDiagonal();
 	btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
-	
-	  btScalar rel_vel = jac.getRelativeVelocity(
+
+	btScalar rel_vel = jac.getRelativeVelocity(
 		body1.getLinearVelocity(),
 		body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
 		body2.getLinearVelocity(),
-		body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
-
-
+		body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
 
 	rel_vel = normal.dot(vel);
-	
+
 	//todo: move this into proper structure
 	btScalar contactDamping = btScalar(0.2);
 
 #ifdef ONLY_USE_LINEAR_MASS
 	btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
-	impulse = - contactDamping * rel_vel * massTerm;
-#else	
+	impulse = -contactDamping * rel_vel * massTerm;
+#else
 	btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
 	impulse = velocityImpulse;
 #endif
 }
-
-
-
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h
index 477c79d1756..255489be991 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h
@@ -22,18 +22,17 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 
 ///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface
-ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class)
+btContactConstraint : public btTypedConstraint
 {
 protected:
-
 	btPersistentManifold m_contactManifold;
 
-public:
-
-
-	btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB);
+protected:
+	btContactConstraint(btPersistentManifold * contactManifold, btRigidBody & rbA, btRigidBody & rbB);
 
-	void	setContactManifold(btPersistentManifold* contactManifold);
+public:
+	void setContactManifold(btPersistentManifold * contactManifold);
 
 	btPersistentManifold* getContactManifold()
 	{
@@ -47,25 +46,20 @@ public:
 
 	virtual ~btContactConstraint();
 
-	virtual void getInfo1 (btConstraintInfo1* info);
+	virtual void getInfo1(btConstraintInfo1 * info);
 
-	virtual void getInfo2 (btConstraintInfo2* info);
+	virtual void getInfo2(btConstraintInfo2 * info);
 
 	///obsolete methods
-	virtual void	buildJacobian();
-
-
+	virtual void buildJacobian();
 };
 
 ///very basic collision resolution without friction
-btScalar resolveSingleCollision(btRigidBody* body1, class btCollisionObject* colObj2, const btVector3& contactPositionWorld,const btVector3& contactNormalOnB, const struct btContactSolverInfo& solverInfo,btScalar distance);
-
+btScalar resolveSingleCollision(btRigidBody* body1, class btCollisionObject* colObj2, const btVector3& contactPositionWorld, const btVector3& contactNormalOnB, const struct btContactSolverInfo& solverInfo, btScalar distance);
 
 ///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects
 void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
-                      btRigidBody& body2, const btVector3& pos2,
-                      btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep);
-
-
+							btRigidBody& body2, const btVector3& pos2,
+							btScalar distance, const btVector3& normal, btScalar& impulse, btScalar timeStep);
 
-#endif //BT_CONTACT_CONSTRAINT_H
+#endif  //BT_CONTACT_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
index a3a0fa6729f..3316403a873 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
@@ -18,7 +18,7 @@ subject to the following restrictions:
 
 #include "LinearMath/btScalar.h"
 
-enum	btSolverMode
+enum btSolverMode
 {
 	SOLVER_RANDMIZE_ORDER = 1,
 	SOLVER_FRICTION_SEPARATE = 2,
@@ -29,131 +29,149 @@ enum	btSolverMode
 	SOLVER_CACHE_FRIENDLY = 128,
 	SOLVER_SIMD = 256,
 	SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
-	SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024
+	SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024,
+	SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048,
+	SOLVER_USE_ARTICULATED_WARMSTARTING = 4096,
 };
 
 struct btContactSolverInfoData
 {
-	
-
-	btScalar	m_tau;
-	btScalar	m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
-	btScalar	m_friction;
-	btScalar	m_timeStep;
-	btScalar	m_restitution;
-	int		m_numIterations;
-	btScalar	m_maxErrorReduction;
-	btScalar	m_sor;
-	btScalar	m_erp;//used as Baumgarte factor
-	btScalar	m_erp2;//used in Split Impulse
-	btScalar	m_globalCfm;//constraint force mixing
-	int			m_splitImpulse;
-	btScalar	m_splitImpulsePenetrationThreshold;
-	btScalar	m_splitImpulseTurnErp;
-	btScalar	m_linearSlop;
-	btScalar	m_warmstartingFactor;
-
-	int			m_solverMode;
-	int	m_restingContactRestitutionThreshold;
-	int			m_minimumSolverBatchSize;
-	btScalar	m_maxGyroscopicForce;
-	btScalar	m_singleAxisRollingFrictionThreshold;
-
-
+	btScalar m_tau;
+	btScalar m_damping;  //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	btScalar m_friction;
+	btScalar m_timeStep;
+	btScalar m_restitution;
+	int m_numIterations;
+	btScalar m_maxErrorReduction;
+	btScalar m_sor;          //successive over-relaxation term
+	btScalar m_erp;          //error reduction for non-contact constraints
+	btScalar m_erp2;         //error reduction for contact constraints
+	btScalar m_deformable_erp;          //error reduction for deformable constraints
+	btScalar m_deformable_cfm;          //constraint force mixing for deformable constraints
+	btScalar m_deformable_maxErrorReduction; // maxErrorReduction for deformable contact
+	btScalar m_globalCfm;    //constraint force mixing for contacts and non-contacts
+	btScalar m_frictionERP;  //error reduction for friction constraints
+	btScalar m_frictionCFM;  //constraint force mixing for friction constraints
+
+	int m_splitImpulse;
+	btScalar m_splitImpulsePenetrationThreshold;
+	btScalar m_splitImpulseTurnErp;
+	btScalar m_linearSlop;
+	btScalar m_warmstartingFactor;
+	btScalar m_articulatedWarmstartingFactor;
+	int m_solverMode;
+	int m_restingContactRestitutionThreshold;
+	int m_minimumSolverBatchSize;
+	btScalar m_maxGyroscopicForce;
+	btScalar m_singleAxisRollingFrictionThreshold;
+	btScalar m_leastSquaresResidualThreshold;
+	btScalar m_restitutionVelocityThreshold;
+	bool m_jointFeedbackInWorldSpace;
+	bool m_jointFeedbackInJointFrame;
+	int m_reportSolverAnalytics;
+	int m_numNonContactInnerIterations;
 };
 
 struct btContactSolverInfo : public btContactSolverInfoData
 {
-
-	
-
 	inline btContactSolverInfo()
 	{
 		m_tau = btScalar(0.6);
 		m_damping = btScalar(1.0);
 		m_friction = btScalar(0.3);
-		m_timeStep = btScalar(1.f/60.f);
+		m_timeStep = btScalar(1.f / 60.f);
 		m_restitution = btScalar(0.);
 		m_maxErrorReduction = btScalar(20.);
 		m_numIterations = 10;
 		m_erp = btScalar(0.2);
-		m_erp2 = btScalar(0.8);
+		m_erp2 = btScalar(0.2);
+		m_deformable_erp = btScalar(0.06);
+		m_deformable_cfm = btScalar(0.01);
+		m_deformable_maxErrorReduction = btScalar(0.1);
 		m_globalCfm = btScalar(0.);
+		m_frictionERP = btScalar(0.2);  //positional friction 'anchors' are disabled by default
+		m_frictionCFM = btScalar(0.);
 		m_sor = btScalar(1.);
 		m_splitImpulse = true;
 		m_splitImpulsePenetrationThreshold = -.04f;
 		m_splitImpulseTurnErp = 0.1f;
 		m_linearSlop = btScalar(0.0);
-		m_warmstartingFactor=btScalar(0.85);
+		m_warmstartingFactor = btScalar(0.85);
+		m_articulatedWarmstartingFactor = btScalar(0.85);
 		//m_solverMode =  SOLVER_USE_WARMSTARTING |  SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER;
-		m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
-		m_restingContactRestitutionThreshold = 2;//unused as of 2.81
-		m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
-		m_maxGyroscopicForce = 100.f; ///it is only used for 'explicit' version of gyroscopic force
-		m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
+		m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;  // | SOLVER_RANDMIZE_ORDER;
+		m_restingContactRestitutionThreshold = 2;              //unused as of 2.81
+		m_minimumSolverBatchSize = 128;                        //try to combine islands until the amount of constraints reaches this limit
+		m_maxGyroscopicForce = 100.f;                          ///it is only used for 'explicit' version of gyroscopic force
+		m_singleAxisRollingFrictionThreshold = 1e30f;          ///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
+		m_leastSquaresResidualThreshold = 0.f;
+		m_restitutionVelocityThreshold = 0.2f;  //if the relative velocity is below this threshold, there is zero restitution
+		m_jointFeedbackInWorldSpace = false;
+		m_jointFeedbackInJointFrame = false;
+		m_reportSolverAnalytics = 0;
+		m_numNonContactInnerIterations = 1;   // the number of inner iterations for solving motor constraint in a single iteration of the constraint solve
 	}
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct btContactSolverInfoDoubleData
 {
-	double		m_tau;
-	double		m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
-	double		m_friction;
-	double		m_timeStep;
-	double		m_restitution;
-	double		m_maxErrorReduction;
-	double		m_sor;
-	double		m_erp;//used as Baumgarte factor
-	double		m_erp2;//used in Split Impulse
-	double		m_globalCfm;//constraint force mixing
-	double		m_splitImpulsePenetrationThreshold;
-	double		m_splitImpulseTurnErp;
-	double		m_linearSlop;
-	double		m_warmstartingFactor;
-	double		m_maxGyroscopicForce;///it is only used for 'explicit' version of gyroscopic force
-	double		m_singleAxisRollingFrictionThreshold;
-
-	int			m_numIterations;
-	int			m_solverMode;
-	int			m_restingContactRestitutionThreshold;
-	int			m_minimumSolverBatchSize;
-	int			m_splitImpulse;
-	char		m_padding[4];
-
+	double m_tau;
+	double m_damping;  //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	double m_friction;
+	double m_timeStep;
+	double m_restitution;
+	double m_maxErrorReduction;
+	double m_sor;
+	double m_erp;        //used as Baumgarte factor
+	double m_erp2;       //used in Split Impulse
+	double m_globalCfm;  //constraint force mixing
+	double m_splitImpulsePenetrationThreshold;
+	double m_splitImpulseTurnErp;
+	double m_linearSlop;
+	double m_warmstartingFactor;
+	double m_articulatedWarmstartingFactor;
+	double m_maxGyroscopicForce;  ///it is only used for 'explicit' version of gyroscopic force
+	double m_singleAxisRollingFrictionThreshold;
+
+	int m_numIterations;
+	int m_solverMode;
+	int m_restingContactRestitutionThreshold;
+	int m_minimumSolverBatchSize;
+	int m_splitImpulse;
+	char m_padding[4];
 };
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct btContactSolverInfoFloatData
 {
-	float		m_tau;
-	float		m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
-	float		m_friction;
-	float		m_timeStep;
-
-	float		m_restitution;
-	float		m_maxErrorReduction;
-	float		m_sor;
-	float		m_erp;//used as Baumgarte factor
-
-	float		m_erp2;//used in Split Impulse
-	float		m_globalCfm;//constraint force mixing
-	float		m_splitImpulsePenetrationThreshold;
-	float		m_splitImpulseTurnErp;
-
-	float		m_linearSlop;
-	float		m_warmstartingFactor;
-	float		m_maxGyroscopicForce;
-	float		m_singleAxisRollingFrictionThreshold;
-
-	int			m_numIterations;
-	int			m_solverMode;
-	int			m_restingContactRestitutionThreshold;
-	int			m_minimumSolverBatchSize;
-
-	int			m_splitImpulse;
-	char		m_padding[4];
+	float m_tau;
+	float m_damping;  //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	float m_friction;
+	float m_timeStep;
+
+	float m_restitution;
+	float m_maxErrorReduction;
+	float m_sor;
+	float m_erp;  //used as Baumgarte factor
+
+	float m_erp2;       //used in Split Impulse
+	float m_globalCfm;  //constraint force mixing
+	float m_splitImpulsePenetrationThreshold;
+	float m_splitImpulseTurnErp;
+
+	float m_linearSlop;
+	float m_warmstartingFactor;
+	float m_articulatedWarmstartingFactor;
+	float m_maxGyroscopicForce;
+
+	float m_singleAxisRollingFrictionThreshold;
+	int m_numIterations;
+	int m_solverMode;
+	int m_restingContactRestitutionThreshold;
+
+	int m_minimumSolverBatchSize;
+	int m_splitImpulse;
+	
 };
 
-
-
-#endif //BT_CONTACT_SOLVER_INFO
+#endif  //BT_CONTACT_SOLVER_INFO
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
index 75d81cc08c2..bba102d9052 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
@@ -13,25 +13,20 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btFixedConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
 #include <new>
 
-
-btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
-:btGeneric6DofSpring2Constraint(rbA,rbB,frameInA,frameInB)
+btFixedConstraint::btFixedConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
+	: btGeneric6DofSpring2Constraint(rbA, rbB, frameInA, frameInB)
 {
-	setAngularLowerLimit(btVector3(0,0,0));
-	setAngularUpperLimit(btVector3(0,0,0));
-	setLinearLowerLimit(btVector3(0,0,0));
-	setLinearUpperLimit(btVector3(0,0,0));
+	setAngularLowerLimit(btVector3(0, 0, 0));
+	setAngularUpperLimit(btVector3(0, 0, 0));
+	setLinearLowerLimit(btVector3(0, 0, 0));
+	setLinearUpperLimit(btVector3(0, 0, 0));
 }
 
-
-
-
-btFixedConstraint::~btFixedConstraint ()
+btFixedConstraint::~btFixedConstraint()
 {
 }
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
index bff2008b283..6d474ea81de 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
@@ -18,16 +18,13 @@ subject to the following restrictions:
 
 #include "btGeneric6DofSpring2Constraint.h"
 
-
-ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btGeneric6DofSpring2Constraint
+ATTRIBUTE_ALIGNED16(class)
+btFixedConstraint : public btGeneric6DofSpring2Constraint
 {
-
 public:
-	btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
+	btFixedConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB);
 
-	
 	virtual ~btFixedConstraint();
-
 };
 
-#endif //BT_FIXED_CONSTRAINT_H
+#endif  //BT_FIXED_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp
index bcd457b6731..7535c52c05f 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp
@@ -17,38 +17,36 @@ subject to the following restrictions:
 
 #include "btGearConstraint.h"
 
-btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio)
-:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB),
-m_axisInA(axisInA),
-m_axisInB(axisInB),
-m_ratio(ratio)
+btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA, const btVector3& axisInB, btScalar ratio)
+	: btTypedConstraint(GEAR_CONSTRAINT_TYPE, rbA, rbB),
+	  m_axisInA(axisInA),
+	  m_axisInB(axisInB),
+	  m_ratio(ratio)
 {
 }
 
-btGearConstraint::~btGearConstraint ()
+btGearConstraint::~btGearConstraint()
 {
 }
 
-void btGearConstraint::getInfo1 (btConstraintInfo1* info)
+void btGearConstraint::getInfo1(btConstraintInfo1* info)
 {
 	info->m_numConstraintRows = 1;
 	info->nub = 1;
 }
 
-void btGearConstraint::getInfo2 (btConstraintInfo2* info)
+void btGearConstraint::getInfo2(btConstraintInfo2* info)
 {
 	btVector3 globalAxisA, globalAxisB;
 
-	globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA;
-	globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB;
+	globalAxisA = m_rbA.getWorldTransform().getBasis() * this->m_axisInA;
+	globalAxisB = m_rbB.getWorldTransform().getBasis() * this->m_axisInB;
 
 	info->m_J1angularAxis[0] = globalAxisA[0];
 	info->m_J1angularAxis[1] = globalAxisA[1];
 	info->m_J1angularAxis[2] = globalAxisA[2];
 
-	info->m_J2angularAxis[0] = m_ratio*globalAxisB[0];
-	info->m_J2angularAxis[1] = m_ratio*globalAxisB[1];
-	info->m_J2angularAxis[2] = m_ratio*globalAxisB[2];
-
+	info->m_J2angularAxis[0] = m_ratio * globalAxisB[0];
+	info->m_J2angularAxis[1] = m_ratio * globalAxisB[1];
+	info->m_J2angularAxis[2] = m_ratio * globalAxisB[2];
 }
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h
index f9afcb91211..64b15dfbce8 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h
@@ -13,45 +13,40 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef BT_GEAR_CONSTRAINT_H
 #define BT_GEAR_CONSTRAINT_H
 
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
 
-
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btGearConstraintData	btGearConstraintDoubleData
-#define btGearConstraintDataName	"btGearConstraintDoubleData"
+#define btGearConstraintData btGearConstraintDoubleData
+#define btGearConstraintDataName "btGearConstraintDoubleData"
 #else
-#define btGearConstraintData	btGearConstraintFloatData
-#define btGearConstraintDataName	"btGearConstraintFloatData"
-#endif //BT_USE_DOUBLE_PRECISION
-
-
+#define btGearConstraintData btGearConstraintFloatData
+#define btGearConstraintDataName "btGearConstraintFloatData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
 ///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio.
 ///See Bullet/Demos/ConstraintDemo for an example use.
 class btGearConstraint : public btTypedConstraint
 {
 protected:
-	btVector3	m_axisInA;
-	btVector3	m_axisInB;
-	bool		m_useFrameA;
-	btScalar	m_ratio;
+	btVector3 m_axisInA;
+	btVector3 m_axisInB;
+	bool m_useFrameA;
+	btScalar m_ratio;
 
 public:
-	btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f);
-	virtual ~btGearConstraint ();
+	btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA, const btVector3& axisInB, btScalar ratio = 1.f);
+	virtual ~btGearConstraint();
 
 	///internal method used by the constraint solver, don't use them directly
-	virtual void getInfo1 (btConstraintInfo1* info);
+	virtual void getInfo1(btConstraintInfo1* info);
 
 	///internal method used by the constraint solver, don't use them directly
-	virtual void getInfo2 (btConstraintInfo2* info);
+	virtual void getInfo2(btConstraintInfo2* info);
 
-	void setAxisA(btVector3& axisA) 
+	void setAxisA(btVector3& axisA)
 	{
 		m_axisInA = axisA;
 	}
@@ -76,77 +71,76 @@ public:
 		return m_ratio;
 	}
 
-
-	virtual	void	setParam(int num, btScalar value, int axis = -1) 
+	virtual void setParam(int num, btScalar value, int axis = -1)
 	{
-		(void) num;
-		(void) value;
-		(void) axis;
+		(void)num;
+		(void)value;
+		(void)axis;
 		btAssert(0);
 	}
 
 	///return the local value of parameter
-	virtual	btScalar getParam(int num, int axis = -1) const 
-	{ 
-		(void) num;
-		(void) axis;
+	virtual btScalar getParam(int num, int axis = -1) const
+	{
+		(void)num;
+		(void)axis;
 		btAssert(0);
 		return 0.f;
 	}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	virtual int calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
-
-
-
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct btGearConstraintFloatData
 {
-	btTypedConstraintFloatData	m_typeConstraintData;
+	btTypedConstraintFloatData m_typeConstraintData;
 
-	btVector3FloatData			m_axisInA;
-	btVector3FloatData			m_axisInB;
+	btVector3FloatData m_axisInA;
+	btVector3FloatData m_axisInB;
 
-	float							m_ratio;
-	char							m_padding[4];
+	float m_ratio;
+	char m_padding[4];
 };
 
 struct btGearConstraintDoubleData
 {
-	btTypedConstraintDoubleData	m_typeConstraintData;
+	btTypedConstraintDoubleData m_typeConstraintData;
 
-	btVector3DoubleData			m_axisInA;
-	btVector3DoubleData			m_axisInB;
+	btVector3DoubleData m_axisInA;
+	btVector3DoubleData m_axisInB;
 
-	double						m_ratio;
+	double m_ratio;
 };
 
-SIMD_FORCE_INLINE	int	btGearConstraint::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btGearConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(btGearConstraintData);
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char* btGearConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 	btGearConstraintData* gear = (btGearConstraintData*)dataBuffer;
-	btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer);
+	btTypedConstraint::serialize(&gear->m_typeConstraintData, serializer);
 
-	m_axisInA.serialize( gear->m_axisInA );
-	m_axisInB.serialize( gear->m_axisInB );
+	m_axisInA.serialize(gear->m_axisInA);
+	m_axisInB.serialize(gear->m_axisInB);
 
 	gear->m_ratio = m_ratio;
 
+	// Fill padding with zeros to appease msan.
+#ifndef BT_USE_DOUBLE_PRECISION
+	gear->m_padding[0] = 0;
+	gear->m_padding[1] = 0;
+	gear->m_padding[2] = 0;
+	gear->m_padding[3] = 0;
+#endif
+
 	return btGearConstraintDataName;
 }
 
-
-
-
-
-
-#endif //BT_GEAR_CONSTRAINT_H
+#endif  //BT_GEAR_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
index bc2b5a85df9..1f542035324 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
@@ -25,83 +25,61 @@ http://gimpact.sf.net
 #include "LinearMath/btTransformUtil.h"
 #include <new>
 
-
-
 #define D6_USE_OBSOLETE_METHOD false
 #define D6_USE_FRAME_OFFSET true
 
-
-
-
-
-
 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
-: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
-, m_frameInA(frameInA)
-, m_frameInB(frameInB),
-m_useLinearReferenceFrameA(useLinearReferenceFrameA),
-m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
-m_flags(0),
-m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
+	: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), m_flags(0), m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
 {
 	calculateTransforms();
 }
 
-
-
 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
-        : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
-		m_frameInB(frameInB),
-		m_useLinearReferenceFrameA(useLinearReferenceFrameB),
-		m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
-		m_flags(0),
-		m_useSolveConstraintObsolete(false)
+	: btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
+	  m_frameInB(frameInB),
+	  m_useLinearReferenceFrameA(useLinearReferenceFrameB),
+	  m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
+	  m_flags(0),
+	  m_useSolveConstraintObsolete(false)
 {
 	///not providing rigidbody A means implicitly using worldspace for body A
 	m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
 	calculateTransforms();
 }
 
-
-
-
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
 
-
-
 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
 {
-	int i = index%3;
-	int j = index/3;
+	int i = index % 3;
+	int j = index / 3;
 	return mat[i][j];
 }
 
-
-
 ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
-bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
-bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
+bool matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz);
+bool matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz)
 {
 	//	// rot =  cy*cz          -cy*sz           sy
 	//	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
 	//	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
 	//
 
-	btScalar fi = btGetMatrixElem(mat,2);
+	btScalar fi = btGetMatrixElem(mat, 2);
 	if (fi < btScalar(1.0f))
 	{
 		if (fi > btScalar(-1.0f))
 		{
-			xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
-			xyz[1] = btAsin(btGetMatrixElem(mat,2));
-			xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+			xyz[0] = btAtan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
+			xyz[1] = btAsin(btGetMatrixElem(mat, 2));
+			xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
 			return true;
 		}
 		else
 		{
 			// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
-			xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+			xyz[0] = -btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
 			xyz[1] = -SIMD_HALF_PI;
 			xyz[2] = btScalar(0.0);
 			return false;
@@ -110,7 +88,7 @@ bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
 	else
 	{
 		// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
-		xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+		xyz[0] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
 		xyz[1] = SIMD_HALF_PI;
 		xyz[2] = 0.0;
 	}
@@ -121,52 +99,49 @@ bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
 
 int btRotationalLimitMotor::testLimitValue(btScalar test_value)
 {
-	if(m_loLimit>m_hiLimit)
+	if (m_loLimit > m_hiLimit)
 	{
-		m_currentLimit = 0;//Free from violation
+		m_currentLimit = 0;  //Free from violation
 		return 0;
 	}
 	if (test_value < m_loLimit)
 	{
-		m_currentLimit = 1;//low limit violation
-		m_currentLimitError =  test_value - m_loLimit;
-		if(m_currentLimitError>SIMD_PI) 
-			m_currentLimitError-=SIMD_2_PI;
-		else if(m_currentLimitError<-SIMD_PI) 
-			m_currentLimitError+=SIMD_2_PI;
+		m_currentLimit = 1;  //low limit violation
+		m_currentLimitError = test_value - m_loLimit;
+		if (m_currentLimitError > SIMD_PI)
+			m_currentLimitError -= SIMD_2_PI;
+		else if (m_currentLimitError < -SIMD_PI)
+			m_currentLimitError += SIMD_2_PI;
 		return 1;
 	}
-	else if (test_value> m_hiLimit)
+	else if (test_value > m_hiLimit)
 	{
-		m_currentLimit = 2;//High limit violation
+		m_currentLimit = 2;  //High limit violation
 		m_currentLimitError = test_value - m_hiLimit;
-		if(m_currentLimitError>SIMD_PI) 
-			m_currentLimitError-=SIMD_2_PI;
-		else if(m_currentLimitError<-SIMD_PI) 
-			m_currentLimitError+=SIMD_2_PI;
+		if (m_currentLimitError > SIMD_PI)
+			m_currentLimitError -= SIMD_2_PI;
+		else if (m_currentLimitError < -SIMD_PI)
+			m_currentLimitError += SIMD_2_PI;
 		return 2;
 	};
 
-	m_currentLimit = 0;//Free from violation
+	m_currentLimit = 0;  //Free from violation
 	return 0;
-
 }
 
-
-
 btScalar btRotationalLimitMotor::solveAngularLimits(
-	btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
-	btRigidBody * body0, btRigidBody * body1 )
+	btScalar timeStep, btVector3& axis, btScalar jacDiagABInv,
+	btRigidBody* body0, btRigidBody* body1)
 {
-	if (needApplyTorques()==false) return 0.0f;
+	if (needApplyTorques() == false) return 0.0f;
 
 	btScalar target_velocity = m_targetVelocity;
 	btScalar maxMotorForce = m_maxMotorForce;
 
 	//current error correction
-	if (m_currentLimit!=0)
+	if (m_currentLimit != 0)
 	{
-		target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
+		target_velocity = -m_stopERP * m_currentLimitError / (timeStep);
 		maxMotorForce = m_maxLimitForce;
 	}
 
@@ -178,42 +153,37 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
 	btVector3 angVelB = body1->getAngularVelocity();
 
 	btVector3 vel_diff;
-	vel_diff = angVelA-angVelB;
-
-
+	vel_diff = angVelA - angVelB;
 
 	btScalar rel_vel = axis.dot(vel_diff);
 
 	// correction velocity
-	btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
+	btScalar motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel);
 
-
-	if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
+	if (motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON)
 	{
-		return 0.0f;//no need for applying force
+		return 0.0f;  //no need for applying force
 	}
 
-
 	// correction impulse
-	btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
+	btScalar unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv;
 
 	// clip correction impulse
 	btScalar clippedMotorImpulse;
 
 	///@todo: should clip against accumulated impulse
-	if (unclippedMotorImpulse>0.0f)
+	if (unclippedMotorImpulse > 0.0f)
 	{
-		clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
+		clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
 	}
 	else
 	{
-		clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
+		clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
 	}
 
-
 	// sort with accumulated impulses
-	btScalar	lo = btScalar(-BT_LARGE_FLOAT);
-	btScalar	hi = btScalar(BT_LARGE_FLOAT);
+	btScalar lo = btScalar(-BT_LARGE_FLOAT);
+	btScalar hi = btScalar(BT_LARGE_FLOAT);
 
 	btScalar oldaccumImpulse = m_accumulatedImpulse;
 	btScalar sum = oldaccumImpulse + clippedMotorImpulse;
@@ -227,59 +197,50 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
 	body1->applyTorqueImpulse(-motorImp);
 
 	return clippedMotorImpulse;
-
-
 }
 
 //////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
 
-
-
-
 //////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
 
-
 int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
 {
 	btScalar loLimit = m_lowerLimit[limitIndex];
 	btScalar hiLimit = m_upperLimit[limitIndex];
-	if(loLimit > hiLimit)
+	if (loLimit > hiLimit)
 	{
-		m_currentLimit[limitIndex] = 0;//Free from violation
+		m_currentLimit[limitIndex] = 0;  //Free from violation
 		m_currentLimitError[limitIndex] = btScalar(0.f);
 		return 0;
 	}
 
 	if (test_value < loLimit)
 	{
-		m_currentLimit[limitIndex] = 2;//low limit violation
-		m_currentLimitError[limitIndex] =  test_value - loLimit;
+		m_currentLimit[limitIndex] = 2;  //low limit violation
+		m_currentLimitError[limitIndex] = test_value - loLimit;
 		return 2;
 	}
-	else if (test_value> hiLimit)
+	else if (test_value > hiLimit)
 	{
-		m_currentLimit[limitIndex] = 1;//High limit violation
+		m_currentLimit[limitIndex] = 1;  //High limit violation
 		m_currentLimitError[limitIndex] = test_value - hiLimit;
 		return 1;
 	};
 
-	m_currentLimit[limitIndex] = 0;//Free from violation
+	m_currentLimit[limitIndex] = 0;  //Free from violation
 	m_currentLimitError[limitIndex] = btScalar(0.f);
 	return 0;
 }
 
-
-
 btScalar btTranslationalLimitMotor::solveLinearAxis(
 	btScalar timeStep,
 	btScalar jacDiagABInv,
-	btRigidBody& body1,const btVector3 &pointInA,
-	btRigidBody& body2,const btVector3 &pointInB,
+	btRigidBody& body1, const btVector3& pointInA,
+	btRigidBody& body2, const btVector3& pointInB,
 	int limit_index,
-	const btVector3 & axis_normal_on_a,
-	const btVector3 & anchorPos)
+	const btVector3& axis_normal_on_a,
+	const btVector3& anchorPos)
 {
-
 	///find relative velocity
 	//    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
 	//    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
@@ -292,14 +253,12 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
 
 	btScalar rel_vel = axis_normal_on_a.dot(vel);
 
-
-
 	/// apply displacement correction
 
 	//positional error (zeroth order error)
 	btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
-	btScalar	lo = btScalar(-BT_LARGE_FLOAT);
-	btScalar	hi = btScalar(BT_LARGE_FLOAT);
+	btScalar lo = btScalar(-BT_LARGE_FLOAT);
+	btScalar hi = btScalar(BT_LARGE_FLOAT);
 
 	btScalar minLimit = m_lowerLimit[limit_index];
 	btScalar maxLimit = m_upperLimit[limit_index];
@@ -312,7 +271,6 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
 			{
 				depth -= maxLimit;
 				lo = btScalar(0.);
-
 			}
 			else
 			{
@@ -329,10 +287,7 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
 		}
 	}
 
-	btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
-
-
-
+	btScalar normalImpulse = m_limitSoftness * (m_restitution * depth / timeStep - m_damping * rel_vel) * jacDiagABInv;
 
 	btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
 	btScalar sum = oldNormalImpulse + normalImpulse;
@@ -340,11 +295,9 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
 	normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
 
 	btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
-	body1.applyImpulse( impulse_vector, rel_pos1);
+	body1.applyImpulse(impulse_vector, rel_pos1);
 	body2.applyImpulse(-impulse_vector, rel_pos2);
 
-	
-
 	return normalImpulse;
 }
 
@@ -352,8 +305,8 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
 
 void btGeneric6DofConstraint::calculateAngleInfo()
 {
-	btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
-	matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
+	btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis();
+	matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff);
 	// in euler angle mode we do not actually constrain the angular velocity
 	// along the axes axis[0] and axis[2] (although we do use axis[1]) :
 	//
@@ -378,31 +331,30 @@ void btGeneric6DofConstraint::calculateAngleInfo()
 	m_calculatedAxis[0].normalize();
 	m_calculatedAxis[1].normalize();
 	m_calculatedAxis[2].normalize();
-
 }
 
 void btGeneric6DofConstraint::calculateTransforms()
 {
-	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 }
 
-void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA, const btTransform& transB)
 {
 	m_calculatedTransformA = transA * m_frameInA;
 	m_calculatedTransformB = transB * m_frameInB;
 	calculateLinearInfo();
 	calculateAngleInfo();
-	if(m_useOffsetForConstraintFrame)
-	{	//  get weight factors depending on masses
+	if (m_useOffsetForConstraintFrame)
+	{  //  get weight factors depending on masses
 		btScalar miA = getRigidBodyA().getInvMass();
 		btScalar miB = getRigidBodyB().getInvMass();
 		m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
 		btScalar miS = miA + miB;
-		if(miS > btScalar(0.f))
+		if (miS > btScalar(0.f))
 		{
 			m_factA = miB / miS;
 		}
-		else 
+		else
 		{
 			m_factA = btScalar(0.5f);
 		}
@@ -410,39 +362,32 @@ void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,cons
 	}
 }
 
-
-
 void btGeneric6DofConstraint::buildLinearJacobian(
-	btJacobianEntry & jacLinear,const btVector3 & normalWorld,
-	const btVector3 & pivotAInW,const btVector3 & pivotBInW)
+	btJacobianEntry& jacLinear, const btVector3& normalWorld,
+	const btVector3& pivotAInW, const btVector3& pivotBInW)
 {
 	new (&jacLinear) btJacobianEntry(
-        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-        pivotAInW - m_rbA.getCenterOfMassPosition(),
-        pivotBInW - m_rbB.getCenterOfMassPosition(),
-        normalWorld,
-        m_rbA.getInvInertiaDiagLocal(),
-        m_rbA.getInvMass(),
-        m_rbB.getInvInertiaDiagLocal(),
-        m_rbB.getInvMass());
+		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+		pivotAInW - m_rbA.getCenterOfMassPosition(),
+		pivotBInW - m_rbB.getCenterOfMassPosition(),
+		normalWorld,
+		m_rbA.getInvInertiaDiagLocal(),
+		m_rbA.getInvMass(),
+		m_rbB.getInvInertiaDiagLocal(),
+		m_rbB.getInvMass());
 }
 
-
-
 void btGeneric6DofConstraint::buildAngularJacobian(
-	btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
+	btJacobianEntry& jacAngular, const btVector3& jointAxisW)
 {
-	 new (&jacAngular)	btJacobianEntry(jointAxisW,
-                                      m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-                                      m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-                                      m_rbA.getInvInertiaDiagLocal(),
-                                      m_rbB.getInvInertiaDiagLocal());
-
+	new (&jacAngular) btJacobianEntry(jointAxisW,
+									  m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+									  m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+									  m_rbA.getInvInertiaDiagLocal(),
+									  m_rbB.getInvInertiaDiagLocal());
 }
 
-
-
 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
 {
 	btScalar angle = m_calculatedAxisAngleDiff[axis_index];
@@ -453,23 +398,20 @@ bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
 	return m_angularLimits[axis_index].needApplyTorques();
 }
 
-
-
 void btGeneric6DofConstraint::buildJacobian()
 {
 #ifndef __SPU__
 	if (m_useSolveConstraintObsolete)
 	{
-
 		// Clear accumulated impulses for the next simulation step
 		m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 		int i;
-		for(i = 0; i < 3; i++)
+		for (i = 0; i < 3; i++)
 		{
 			m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
 		}
 		//calculates transform
-		calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+		calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 
 		//  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
 		//  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
@@ -483,7 +425,7 @@ void btGeneric6DofConstraint::buildJacobian()
 
 		btVector3 normalWorld;
 		//linear part
-		for (i=0;i<3;i++)
+		for (i = 0; i < 3; i++)
 		{
 			if (m_linearLimits.isLimited(i))
 			{
@@ -493,56 +435,53 @@ void btGeneric6DofConstraint::buildJacobian()
 					normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
 
 				buildLinearJacobian(
-					m_jacLinear[i],normalWorld ,
-					pivotAInW,pivotBInW);
-
+					m_jacLinear[i], normalWorld,
+					pivotAInW, pivotBInW);
 			}
 		}
 
 		// angular part
-		for (i=0;i<3;i++)
+		for (i = 0; i < 3; i++)
 		{
 			//calculates error angle
 			if (testAngularLimitMotor(i))
 			{
 				normalWorld = this->getAxis(i);
 				// Create angular atom
-				buildAngularJacobian(m_jacAng[i],normalWorld);
+				buildAngularJacobian(m_jacAng[i], normalWorld);
 			}
 		}
-
 	}
-#endif //__SPU__
-
+#endif  //__SPU__
 }
 
-
-void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
+void btGeneric6DofConstraint::getInfo1(btConstraintInfo1* info)
 {
 	if (m_useSolveConstraintObsolete)
 	{
 		info->m_numConstraintRows = 0;
 		info->nub = 0;
-	} else
+	}
+	else
 	{
 		//prepare constraint
-		calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+		calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 		info->m_numConstraintRows = 0;
 		info->nub = 6;
 		int i;
 		//test linear limits
-		for(i = 0; i < 3; i++)
+		for (i = 0; i < 3; i++)
 		{
-			if(m_linearLimits.needApplyForce(i))
+			if (m_linearLimits.needApplyForce(i))
 			{
 				info->m_numConstraintRows++;
 				info->nub--;
 			}
 		}
 		//test angular limits
-		for (i=0;i<3 ;i++ )
+		for (i = 0; i < 3; i++)
 		{
-			if(testAngularLimitMotor(i))
+			if (testAngularLimitMotor(i))
 			{
 				info->m_numConstraintRows++;
 				info->nub--;
@@ -551,13 +490,14 @@ void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
 	}
 }
 
-void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+void btGeneric6DofConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
 {
 	if (m_useSolveConstraintObsolete)
 	{
 		info->m_numConstraintRows = 0;
 		info->nub = 0;
-	} else
+	}
+	else
 	{
 		//pre-allocate all 6
 		info->m_numConstraintRows = 6;
@@ -565,8 +505,7 @@ void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
 	}
 }
 
-
-void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
+void btGeneric6DofConstraint::getInfo2(btConstraintInfo2* info)
 {
 	btAssert(!m_useSolveConstraintObsolete);
 
@@ -577,136 +516,124 @@ void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
 	const btVector3& angVelA = m_rbA.getAngularVelocity();
 	const btVector3& angVelB = m_rbB.getAngularVelocity();
 
-	if(m_useOffsetForConstraintFrame)
-	{ // for stability better to solve angular limits first
-		int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
-		setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
+	if (m_useOffsetForConstraintFrame)
+	{  // for stability better to solve angular limits first
+		int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 	}
 	else
-	{ // leave old version for compatibility
-		int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
-		setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
+	{  // leave old version for compatibility
+		int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 	}
-
 }
 
-
-void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+void btGeneric6DofConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
 {
-	
 	btAssert(!m_useSolveConstraintObsolete);
 	//prepare constraint
-	calculateTransforms(transA,transB);
+	calculateTransforms(transA, transB);
 
 	int i;
-	for (i=0;i<3 ;i++ )
+	for (i = 0; i < 3; i++)
 	{
 		testAngularLimitMotor(i);
 	}
 
-	if(m_useOffsetForConstraintFrame)
-	{ // for stability better to solve angular limits first
-		int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
-		setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
+	if (m_useOffsetForConstraintFrame)
+	{  // for stability better to solve angular limits first
+		int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 	}
 	else
-	{ // leave old version for compatibility
-		int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
-		setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
+	{  // leave old version for compatibility
+		int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 	}
 }
 
-
-
-int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
 {
-//	int row = 0;
+	//	int row = 0;
 	//solve linear limits
 	btRotationalLimitMotor limot;
-	for (int i=0;i<3 ;i++ )
+	for (int i = 0; i < 3; i++)
 	{
-		if(m_linearLimits.needApplyForce(i))
-		{ // re-use rotational motor code
+		if (m_linearLimits.needApplyForce(i))
+		{  // re-use rotational motor code
 			limot.m_bounce = btScalar(0.f);
 			limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
 			limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
-			limot.m_currentLimitError  = m_linearLimits.m_currentLimitError[i];
-			limot.m_damping  = m_linearLimits.m_damping;
-			limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
-			limot.m_hiLimit  = m_linearLimits.m_upperLimit[i];
-			limot.m_limitSoftness  = m_linearLimits.m_limitSoftness;
-			limot.m_loLimit  = m_linearLimits.m_lowerLimit[i];
-			limot.m_maxLimitForce  = btScalar(0.f);
-			limot.m_maxMotorForce  = m_linearLimits.m_maxMotorForce[i];
-			limot.m_targetVelocity  = m_linearLimits.m_targetVelocity[i];
+			limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
+			limot.m_damping = m_linearLimits.m_damping;
+			limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
+			limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
+			limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
+			limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
+			limot.m_maxLimitForce = btScalar(0.f);
+			limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
+			limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
 			btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
 			int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
-			limot.m_normalCFM	= (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
-			limot.m_stopCFM		= (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
-			limot.m_stopERP		= (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
-			if(m_useOffsetForConstraintFrame)
+			limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
+			limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
+			limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
+			if (m_useOffsetForConstraintFrame)
 			{
 				int indx1 = (i + 1) % 3;
 				int indx2 = (i + 2) % 3;
-				int rotAllowed = 1; // rotations around orthos to current axis
-				if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
+				int rotAllowed = 1;  // rotations around orthos to current axis
+				if (m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
 				{
 					rotAllowed = 0;
 				}
-				row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
+				row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
 			}
 			else
 			{
-				row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
+				row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0);
 			}
 		}
 	}
 	return row;
 }
 
-
-
-int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2* info, int row_offset, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
 {
-	btGeneric6DofConstraint * d6constraint = this;
+	btGeneric6DofConstraint* d6constraint = this;
 	int row = row_offset;
 	//solve angular limits
-	for (int i=0;i<3 ;i++ )
+	for (int i = 0; i < 3; i++)
 	{
-		if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
+		if (d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
 		{
 			btVector3 axis = d6constraint->getAxis(i);
 			int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
-			if(!(flags & BT_6DOF_FLAGS_CFM_NORM))
+			if (!(flags & BT_6DOF_FLAGS_CFM_NORM))
 			{
 				m_angularLimits[i].m_normalCFM = info->cfm[0];
 			}
-			if(!(flags & BT_6DOF_FLAGS_CFM_STOP))
+			if (!(flags & BT_6DOF_FLAGS_CFM_STOP))
 			{
 				m_angularLimits[i].m_stopCFM = info->cfm[0];
 			}
-			if(!(flags & BT_6DOF_FLAGS_ERP_STOP))
+			if (!(flags & BT_6DOF_FLAGS_ERP_STOP))
 			{
 				m_angularLimits[i].m_stopERP = info->erp;
 			}
 			row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
-												transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
+										 transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
 		}
 	}
 
 	return row;
 }
 
-
-
-
-void	btGeneric6DofConstraint::updateRHS(btScalar	timeStep)
+void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
 {
 	(void)timeStep;
-
 }
 
-
 void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
 {
 	m_frameInA = frameA;
@@ -715,33 +642,27 @@ void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTrans
 	calculateTransforms();
 }
 
-
-
 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
 {
 	return m_calculatedAxis[axis_index];
 }
 
-
-btScalar	btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
+btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
 {
 	return m_calculatedLinearDiff[axisIndex];
 }
 
-
 btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
 {
 	return m_calculatedAxisAngleDiff[axisIndex];
 }
 
-
-
 void btGeneric6DofConstraint::calcAnchorPos(void)
 {
 	btScalar imA = m_rbA.getInvMass();
 	btScalar imB = m_rbB.getInvMass();
 	btScalar weight;
-	if(imB == btScalar(0.0))
+	if (imB == btScalar(0.0))
 	{
 		weight = btScalar(1.0);
 	}
@@ -755,43 +676,39 @@ void btGeneric6DofConstraint::calcAnchorPos(void)
 	return;
 }
 
-
-
 void btGeneric6DofConstraint::calculateLinearInfo()
 {
 	m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
 	m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
-	for(int i = 0; i < 3; i++)
+	for (int i = 0; i < 3; i++)
 	{
 		m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
 		m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
 	}
 }
 
-
-
 int btGeneric6DofConstraint::get_limit_motor_info2(
-	btRotationalLimitMotor * limot,
-	const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
-	btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
-{
-    int srow = row * info->rowskip;
-    int powered = limot->m_enableMotor;
-    int limit = limot->m_currentLimit;
-    if (powered || limit)
-    {   // if the joint is powered, or has joint limits, add in the extra row
-        btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
-        btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
-        J1[srow+0] = ax1[0];
-        J1[srow+1] = ax1[1];
-        J1[srow+2] = ax1[2];
-
-        J2[srow+0] = -ax1[0];
-        J2[srow+1] = -ax1[1];
-        J2[srow+2] = -ax1[2];
-
-		if((!rotational))
-        {
+	btRotationalLimitMotor* limot,
+	const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB,
+	btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed)
+{
+	int srow = row * info->rowskip;
+	bool powered = limot->m_enableMotor;
+	int limit = limot->m_currentLimit;
+	if (powered || limit)
+	{  // if the joint is powered, or has joint limits, add in the extra row
+		btScalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
+		btScalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
+		J1[srow + 0] = ax1[0];
+		J1[srow + 1] = ax1[1];
+		J1[srow + 2] = ax1[2];
+
+		J2[srow + 0] = -ax1[0];
+		J2[srow + 1] = -ax1[1];
+		J2[srow + 2] = -ax1[2];
+
+		if ((!rotational))
+		{
 			if (m_useOffsetForConstraintFrame)
 			{
 				btVector3 tmpA, tmpB, relA, relB;
@@ -814,55 +731,56 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
 				relB = orthoB - totalDist * m_factB;
 				tmpA = relA.cross(ax1);
 				tmpB = relB.cross(ax1);
-				if(m_hasStaticBody && (!rotAllowed))
+				if (m_hasStaticBody && (!rotAllowed))
 				{
 					tmpA *= m_factA;
 					tmpB *= m_factB;
 				}
 				int i;
-				for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
-				for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
-			} else
+				for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
+				for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
+			}
+			else
 			{
-				btVector3 ltd;	// Linear Torque Decoupling vector
+				btVector3 ltd;  // Linear Torque Decoupling vector
 				btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
 				ltd = c.cross(ax1);
-				info->m_J1angularAxis[srow+0] = ltd[0];
-				info->m_J1angularAxis[srow+1] = ltd[1];
-				info->m_J1angularAxis[srow+2] = ltd[2];
+				info->m_J1angularAxis[srow + 0] = ltd[0];
+				info->m_J1angularAxis[srow + 1] = ltd[1];
+				info->m_J1angularAxis[srow + 2] = ltd[2];
 
 				c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
 				ltd = -c.cross(ax1);
-				info->m_J2angularAxis[srow+0] = ltd[0];
-				info->m_J2angularAxis[srow+1] = ltd[1];
-				info->m_J2angularAxis[srow+2] = ltd[2];
+				info->m_J2angularAxis[srow + 0] = ltd[0];
+				info->m_J2angularAxis[srow + 1] = ltd[1];
+				info->m_J2angularAxis[srow + 2] = ltd[2];
 			}
-        }
-        // if we're limited low and high simultaneously, the joint motor is
-        // ineffective
-        if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
-        info->m_constraintError[srow] = btScalar(0.f);
-        if (powered)
-        {
+		}
+		// if we're limited low and high simultaneously, the joint motor is
+		// ineffective
+		if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
+		info->m_constraintError[srow] = btScalar(0.f);
+		if (powered)
+		{
 			info->cfm[srow] = limot->m_normalCFM;
-            if(!limit)
-            {
+			if (!limit)
+			{
 				btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
 
-				btScalar mot_fact = getMotorFactor(	limot->m_currentPosition, 
-													limot->m_loLimit,
-													limot->m_hiLimit, 
-													tag_vel, 
-													info->fps * limot->m_stopERP);
+				btScalar mot_fact = getMotorFactor(limot->m_currentPosition,
+												   limot->m_loLimit,
+												   limot->m_hiLimit,
+												   tag_vel,
+												   info->fps * limot->m_stopERP);
 				info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
-                info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
-                info->m_upperLimit[srow] = limot->m_maxMotorForce;
-            }
-        }
-        if(limit)
-        {
-            btScalar k = info->fps * limot->m_stopERP;
-			if(!rotational)
+				info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
+				info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
+			}
+		}
+		if (limit)
+		{
+			btScalar k = info->fps * limot->m_stopERP;
+			if (!rotational)
 			{
 				info->m_constraintError[srow] += k * limot->m_currentLimitError;
 			}
@@ -871,116 +789,112 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
 				info->m_constraintError[srow] += -k * limot->m_currentLimitError;
 			}
 			info->cfm[srow] = limot->m_stopCFM;
-            if (limot->m_loLimit == limot->m_hiLimit)
-            {   // limited low and high simultaneously
-                info->m_lowerLimit[srow] = -SIMD_INFINITY;
-                info->m_upperLimit[srow] = SIMD_INFINITY;
-            }
-            else
-            {
-                if (limit == 1)
-                {
-                    info->m_lowerLimit[srow] = 0;
-                    info->m_upperLimit[srow] = SIMD_INFINITY;
-                }
-                else
-                {
-                    info->m_lowerLimit[srow] = -SIMD_INFINITY;
-                    info->m_upperLimit[srow] = 0;
-                }
-                // deal with bounce
-                if (limot->m_bounce > 0)
-                {
-                    // calculate joint velocity
-                    btScalar vel;
-                    if (rotational)
-                    {
-                        vel = angVelA.dot(ax1);
-//make sure that if no body -> angVelB == zero vec
-//                        if (body1)
-                            vel -= angVelB.dot(ax1);
-                    }
-                    else
-                    {
-                        vel = linVelA.dot(ax1);
-//make sure that if no body -> angVelB == zero vec
-//                        if (body1)
-                            vel -= linVelB.dot(ax1);
-                    }
-                    // only apply bounce if the velocity is incoming, and if the
-                    // resulting c[] exceeds what we already have.
-                    if (limit == 1)
-                    {
-                        if (vel < 0)
-                        {
-                            btScalar newc = -limot->m_bounce* vel;
-                            if (newc > info->m_constraintError[srow]) 
+			if (limot->m_loLimit == limot->m_hiLimit)
+			{  // limited low and high simultaneously
+				info->m_lowerLimit[srow] = -SIMD_INFINITY;
+				info->m_upperLimit[srow] = SIMD_INFINITY;
+			}
+			else
+			{
+				if (limit == 1)
+				{
+					info->m_lowerLimit[srow] = 0;
+					info->m_upperLimit[srow] = SIMD_INFINITY;
+				}
+				else
+				{
+					info->m_lowerLimit[srow] = -SIMD_INFINITY;
+					info->m_upperLimit[srow] = 0;
+				}
+				// deal with bounce
+				if (limot->m_bounce > 0)
+				{
+					// calculate joint velocity
+					btScalar vel;
+					if (rotational)
+					{
+						vel = angVelA.dot(ax1);
+						//make sure that if no body -> angVelB == zero vec
+						//                        if (body1)
+						vel -= angVelB.dot(ax1);
+					}
+					else
+					{
+						vel = linVelA.dot(ax1);
+						//make sure that if no body -> angVelB == zero vec
+						//                        if (body1)
+						vel -= linVelB.dot(ax1);
+					}
+					// only apply bounce if the velocity is incoming, and if the
+					// resulting c[] exceeds what we already have.
+					if (limit == 1)
+					{
+						if (vel < 0)
+						{
+							btScalar newc = -limot->m_bounce * vel;
+							if (newc > info->m_constraintError[srow])
 								info->m_constraintError[srow] = newc;
-                        }
-                    }
-                    else
-                    {
-                        if (vel > 0)
-                        {
-                            btScalar newc = -limot->m_bounce * vel;
-                            if (newc < info->m_constraintError[srow]) 
+						}
+					}
+					else
+					{
+						if (vel > 0)
+						{
+							btScalar newc = -limot->m_bounce * vel;
+							if (newc < info->m_constraintError[srow])
 								info->m_constraintError[srow] = newc;
-                        }
-                    }
-                }
-            }
-        }
-        return 1;
-    }
-    else return 0;
+						}
+					}
+				}
+			}
+		}
+		return 1;
+	}
+	else
+		return 0;
 }
 
-
-
-
-
-
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
-	///If no axis is provided, it uses the default axis for this constraint.
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+///If no axis is provided, it uses the default axis for this constraint.
 void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
 {
-	if((axis >= 0) && (axis < 3))
+	if ((axis >= 0) && (axis < 3))
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_STOP_ERP : 
+			case BT_CONSTRAINT_STOP_ERP:
 				m_linearLimits.m_stopERP[axis] = value;
 				m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			case BT_CONSTRAINT_STOP_CFM : 
+			case BT_CONSTRAINT_STOP_CFM:
 				m_linearLimits.m_stopCFM[axis] = value;
 				m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			case BT_CONSTRAINT_CFM : 
+			case BT_CONSTRAINT_CFM:
 				m_linearLimits.m_normalCFM[axis] = value;
 				m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			default : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
-	else if((axis >=3) && (axis < 6))
+	else if ((axis >= 3) && (axis < 6))
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_STOP_ERP : 
+			case BT_CONSTRAINT_STOP_ERP:
 				m_angularLimits[axis - 3].m_stopERP = value;
 				m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			case BT_CONSTRAINT_STOP_CFM : 
+			case BT_CONSTRAINT_STOP_CFM:
 				m_angularLimits[axis - 3].m_stopCFM = value;
 				m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			case BT_CONSTRAINT_CFM : 
+			case BT_CONSTRAINT_CFM:
 				m_angularLimits[axis - 3].m_normalCFM = value;
 				m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			default : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
@@ -990,47 +904,47 @@ void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
 	}
 }
 
-	///return the local value of parameter
-btScalar btGeneric6DofConstraint::getParam(int num, int axis) const 
+///return the local value of parameter
+btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
 {
 	btScalar retVal = 0;
-	if((axis >= 0) && (axis < 3))
+	if ((axis >= 0) && (axis < 3))
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_STOP_ERP : 
+			case BT_CONSTRAINT_STOP_ERP:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_linearLimits.m_stopERP[axis];
 				break;
-			case BT_CONSTRAINT_STOP_CFM : 
+			case BT_CONSTRAINT_STOP_CFM:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_linearLimits.m_stopCFM[axis];
 				break;
-			case BT_CONSTRAINT_CFM : 
+			case BT_CONSTRAINT_CFM:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_linearLimits.m_normalCFM[axis];
 				break;
-			default : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
-	else if((axis >=3) && (axis < 6))
+	else if ((axis >= 3) && (axis < 6))
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_STOP_ERP : 
+			case BT_CONSTRAINT_STOP_ERP:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_angularLimits[axis - 3].m_stopERP;
 				break;
-			case BT_CONSTRAINT_STOP_CFM : 
+			case BT_CONSTRAINT_STOP_CFM:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_angularLimits[axis - 3].m_stopCFM;
 				break;
-			case BT_CONSTRAINT_CFM : 
+			case BT_CONSTRAINT_CFM:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_angularLimits[axis - 3].m_normalCFM;
 				break;
-			default : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
@@ -1041,23 +955,21 @@ btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
 	return retVal;
 }
 
- 
-
-void btGeneric6DofConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
+void btGeneric6DofConstraint::setAxis(const btVector3& axis1, const btVector3& axis2)
 {
 	btVector3 zAxis = axis1.normalized();
 	btVector3 yAxis = axis2.normalized();
-	btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
-	
+	btVector3 xAxis = yAxis.cross(zAxis);  // we want right coordinate system
+
 	btTransform frameInW;
 	frameInW.setIdentity();
-	frameInW.getBasis().setValue(	xAxis[0], yAxis[0], zAxis[0],	
-	                                xAxis[1], yAxis[1], zAxis[1],
-	                               xAxis[2], yAxis[2], zAxis[2]);
-	
+	frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
+								 xAxis[1], yAxis[1], zAxis[1],
+								 xAxis[2], yAxis[2], zAxis[2]);
+
 	// now get constraint frame in local coordinate systems
 	m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
 	m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
-	
+
 	calculateTransforms();
 }
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
index bea8629c325..b9e762e175f 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
@@ -23,7 +23,6 @@ email: projectileman@yahoo.com
 http://gimpact.sf.net
 */
 
-
 #ifndef BT_GENERIC_6DOF_CONSTRAINT_H
 #define BT_GENERIC_6DOF_CONSTRAINT_H
 
@@ -33,96 +32,91 @@ http://gimpact.sf.net
 
 class btRigidBody;
 
-
-
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btGeneric6DofConstraintData2		btGeneric6DofConstraintDoubleData2
-#define btGeneric6DofConstraintDataName	"btGeneric6DofConstraintDoubleData2"
+#define btGeneric6DofConstraintData2 btGeneric6DofConstraintDoubleData2
+#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintDoubleData2"
 #else
-#define btGeneric6DofConstraintData2		btGeneric6DofConstraintData
-#define btGeneric6DofConstraintDataName	"btGeneric6DofConstraintData"
-#endif //BT_USE_DOUBLE_PRECISION
-
+#define btGeneric6DofConstraintData2 btGeneric6DofConstraintData
+#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
 //! Rotation Limit structure for generic joints
 class btRotationalLimitMotor
 {
 public:
-    //! limit_parameters
-    //!@{
-    btScalar m_loLimit;//!< joint limit
-    btScalar m_hiLimit;//!< joint limit
-    btScalar m_targetVelocity;//!< target motor velocity
-    btScalar m_maxMotorForce;//!< max force on motor
-    btScalar m_maxLimitForce;//!< max force on limit
-    btScalar m_damping;//!< Damping.
-    btScalar m_limitSoftness;//! Relaxation factor
-    btScalar m_normalCFM;//!< Constraint force mixing factor
-    btScalar m_stopERP;//!< Error tolerance factor when joint is at limit
-    btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
-    btScalar m_bounce;//!< restitution factor
-    bool m_enableMotor;
-
-    //!@}
-
-    //! temp_variables
-    //!@{
-    btScalar m_currentLimitError;//!  How much is violated this limit
-    btScalar m_currentPosition;     //!  current value of angle 
-    int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
-    btScalar m_accumulatedImpulse;
-    //!@}
-
-    btRotationalLimitMotor()
-    {
-    	m_accumulatedImpulse = 0.f;
-        m_targetVelocity = 0;
-        m_maxMotorForce = 0.1f;
-        m_maxLimitForce = 300.0f;
-        m_loLimit = 1.0f;
-        m_hiLimit = -1.0f;
+	//! limit_parameters
+	//!@{
+	btScalar m_loLimit;         //!< joint limit
+	btScalar m_hiLimit;         //!< joint limit
+	btScalar m_targetVelocity;  //!< target motor velocity
+	btScalar m_maxMotorForce;   //!< max force on motor
+	btScalar m_maxLimitForce;   //!< max force on limit
+	btScalar m_damping;         //!< Damping.
+	btScalar m_limitSoftness;   //! Relaxation factor
+	btScalar m_normalCFM;       //!< Constraint force mixing factor
+	btScalar m_stopERP;         //!< Error tolerance factor when joint is at limit
+	btScalar m_stopCFM;         //!< Constraint force mixing factor when joint is at limit
+	btScalar m_bounce;          //!< restitution factor
+	bool m_enableMotor;
+
+	//!@}
+
+	//! temp_variables
+	//!@{
+	btScalar m_currentLimitError;  //!  How much is violated this limit
+	btScalar m_currentPosition;    //!  current value of angle
+	int m_currentLimit;            //!< 0=free, 1=at lo limit, 2=at hi limit
+	btScalar m_accumulatedImpulse;
+	//!@}
+
+	btRotationalLimitMotor()
+	{
+		m_accumulatedImpulse = 0.f;
+		m_targetVelocity = 0;
+		m_maxMotorForce = 6.0f;
+		m_maxLimitForce = 300.0f;
+		m_loLimit = 1.0f;
+		m_hiLimit = -1.0f;
 		m_normalCFM = 0.f;
 		m_stopERP = 0.2f;
 		m_stopCFM = 0.f;
-        m_bounce = 0.0f;
-        m_damping = 1.0f;
-        m_limitSoftness = 0.5f;
-        m_currentLimit = 0;
-        m_currentLimitError = 0;
-        m_enableMotor = false;
-    }
-
-    btRotationalLimitMotor(const btRotationalLimitMotor & limot)
-    {
-        m_targetVelocity = limot.m_targetVelocity;
-        m_maxMotorForce = limot.m_maxMotorForce;
-        m_limitSoftness = limot.m_limitSoftness;
-        m_loLimit = limot.m_loLimit;
-        m_hiLimit = limot.m_hiLimit;
+		m_bounce = 0.0f;
+		m_damping = 1.0f;
+		m_limitSoftness = 0.5f;
+		m_currentLimit = 0;
+		m_currentLimitError = 0;
+		m_enableMotor = false;
+	}
+
+	btRotationalLimitMotor(const btRotationalLimitMotor& limot)
+	{
+		m_targetVelocity = limot.m_targetVelocity;
+		m_maxMotorForce = limot.m_maxMotorForce;
+		m_limitSoftness = limot.m_limitSoftness;
+		m_loLimit = limot.m_loLimit;
+		m_hiLimit = limot.m_hiLimit;
 		m_normalCFM = limot.m_normalCFM;
 		m_stopERP = limot.m_stopERP;
-		m_stopCFM =	limot.m_stopCFM;
-        m_bounce = limot.m_bounce;
-        m_currentLimit = limot.m_currentLimit;
-        m_currentLimitError = limot.m_currentLimitError;
-        m_enableMotor = limot.m_enableMotor;
-    }
-
-
+		m_stopCFM = limot.m_stopCFM;
+		m_bounce = limot.m_bounce;
+		m_currentLimit = limot.m_currentLimit;
+		m_currentLimitError = limot.m_currentLimitError;
+		m_enableMotor = limot.m_enableMotor;
+	}
 
 	//! Is limited
-    bool isLimited() const
-    {
-    	if(m_loLimit > m_hiLimit) return false;
-    	return true;
-    }
+	bool isLimited() const
+	{
+		if (m_loLimit > m_hiLimit) return false;
+		return true;
+	}
 
 	//! Need apply correction
-    bool needApplyTorques() const
-    {
-    	if(m_currentLimit == 0 && m_enableMotor == false) return false;
-    	return true;
-    }
+	bool needApplyTorques() const
+	{
+		if (m_currentLimit == 0 && m_enableMotor == false) return false;
+		return true;
+	}
 
 	//! calculates  error
 	/*!
@@ -131,104 +125,98 @@ public:
 	int testLimitValue(btScalar test_value);
 
 	//! apply the correction impulses for two bodies
-    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
-
+	btScalar solveAngularLimits(btScalar timeStep, btVector3& axis, btScalar jacDiagABInv, btRigidBody* body0, btRigidBody* body1);
 };
 
-
-
 class btTranslationalLimitMotor
 {
 public:
-	btVector3 m_lowerLimit;//!< the constraint lower limits
-    btVector3 m_upperLimit;//!< the constraint upper limits
-    btVector3 m_accumulatedImpulse;
-    //! Linear_Limit_parameters
-    //!@{
-    btScalar	m_limitSoftness;//!< Softness for linear limit
-    btScalar	m_damping;//!< Damping for linear limit
-    btScalar	m_restitution;//! Bounce parameter for linear limit
-	btVector3	m_normalCFM;//!< Constraint force mixing factor
-    btVector3	m_stopERP;//!< Error tolerance factor when joint is at limit
-	btVector3	m_stopCFM;//!< Constraint force mixing factor when joint is at limit
-    //!@}
-	bool		m_enableMotor[3];
-    btVector3	m_targetVelocity;//!< target motor velocity
-    btVector3	m_maxMotorForce;//!< max force on motor
-    btVector3	m_currentLimitError;//!  How much is violated this limit
-    btVector3	m_currentLinearDiff;//!  Current relative offset of constraint frames
-    int			m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
-
-    btTranslationalLimitMotor()
-    {
-    	m_lowerLimit.setValue(0.f,0.f,0.f);
-    	m_upperLimit.setValue(0.f,0.f,0.f);
-    	m_accumulatedImpulse.setValue(0.f,0.f,0.f);
+	btVector3 m_lowerLimit;  //!< the constraint lower limits
+	btVector3 m_upperLimit;  //!< the constraint upper limits
+	btVector3 m_accumulatedImpulse;
+	//! Linear_Limit_parameters
+	//!@{
+	btScalar m_limitSoftness;  //!< Softness for linear limit
+	btScalar m_damping;        //!< Damping for linear limit
+	btScalar m_restitution;    //! Bounce parameter for linear limit
+	btVector3 m_normalCFM;     //!< Constraint force mixing factor
+	btVector3 m_stopERP;       //!< Error tolerance factor when joint is at limit
+	btVector3 m_stopCFM;       //!< Constraint force mixing factor when joint is at limit
+							   //!@}
+	bool m_enableMotor[3];
+	btVector3 m_targetVelocity;     //!< target motor velocity
+	btVector3 m_maxMotorForce;      //!< max force on motor
+	btVector3 m_currentLimitError;  //!  How much is violated this limit
+	btVector3 m_currentLinearDiff;  //!  Current relative offset of constraint frames
+	int m_currentLimit[3];          //!< 0=free, 1=at lower limit, 2=at upper limit
+
+	btTranslationalLimitMotor()
+	{
+		m_lowerLimit.setValue(0.f, 0.f, 0.f);
+		m_upperLimit.setValue(0.f, 0.f, 0.f);
+		m_accumulatedImpulse.setValue(0.f, 0.f, 0.f);
 		m_normalCFM.setValue(0.f, 0.f, 0.f);
 		m_stopERP.setValue(0.2f, 0.2f, 0.2f);
 		m_stopCFM.setValue(0.f, 0.f, 0.f);
 
-    	m_limitSoftness = 0.7f;
-    	m_damping = btScalar(1.0f);
-    	m_restitution = btScalar(0.5f);
-		for(int i=0; i < 3; i++) 
+		m_limitSoftness = 0.7f;
+		m_damping = btScalar(1.0f);
+		m_restitution = btScalar(0.5f);
+		for (int i = 0; i < 3; i++)
 		{
 			m_enableMotor[i] = false;
 			m_targetVelocity[i] = btScalar(0.f);
 			m_maxMotorForce[i] = btScalar(0.f);
 		}
-    }
+	}
 
-    btTranslationalLimitMotor(const btTranslationalLimitMotor & other )
-    {
-    	m_lowerLimit = other.m_lowerLimit;
-    	m_upperLimit = other.m_upperLimit;
-    	m_accumulatedImpulse = other.m_accumulatedImpulse;
+	btTranslationalLimitMotor(const btTranslationalLimitMotor& other)
+	{
+		m_lowerLimit = other.m_lowerLimit;
+		m_upperLimit = other.m_upperLimit;
+		m_accumulatedImpulse = other.m_accumulatedImpulse;
 
-    	m_limitSoftness = other.m_limitSoftness ;
-    	m_damping = other.m_damping;
-    	m_restitution = other.m_restitution;
+		m_limitSoftness = other.m_limitSoftness;
+		m_damping = other.m_damping;
+		m_restitution = other.m_restitution;
 		m_normalCFM = other.m_normalCFM;
 		m_stopERP = other.m_stopERP;
 		m_stopCFM = other.m_stopCFM;
 
-		for(int i=0; i < 3; i++) 
+		for (int i = 0; i < 3; i++)
 		{
 			m_enableMotor[i] = other.m_enableMotor[i];
 			m_targetVelocity[i] = other.m_targetVelocity[i];
 			m_maxMotorForce[i] = other.m_maxMotorForce[i];
 		}
-    }
+	}
 
-    //! Test limit
+	//! Test limit
 	/*!
     - free means upper < lower,
     - locked means upper == lower
     - limited means upper > lower
     - limitIndex: first 3 are linear, next 3 are angular
     */
-    inline bool	isLimited(int limitIndex) const
-    {
-       return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
-    }
-    inline bool needApplyForce(int limitIndex) const
-    {
-    	if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
-    	return true;
-    }
+	inline bool isLimited(int limitIndex) const
+	{
+		return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
+	}
+	inline bool needApplyForce(int limitIndex) const
+	{
+		if (m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
+		return true;
+	}
 	int testLimitValue(int limitIndex, btScalar test_value);
 
-
-    btScalar solveLinearAxis(
-    	btScalar timeStep,
-        btScalar jacDiagABInv,
-        btRigidBody& body1,const btVector3 &pointInA,
-        btRigidBody& body2,const btVector3 &pointInB,
-        int limit_index,
-        const btVector3 & axis_normal_on_a,
-		const btVector3 & anchorPos);
-
-
+	btScalar solveLinearAxis(
+		btScalar timeStep,
+		btScalar jacDiagABInv,
+		btRigidBody& body1, const btVector3& pointInA,
+		btRigidBody& body2, const btVector3& pointInB,
+		int limit_index,
+		const btVector3& axis_normal_on_a,
+		const btVector3& anchorPos);
 };
 
 enum bt6DofFlags
@@ -237,8 +225,7 @@ enum bt6DofFlags
 	BT_6DOF_FLAGS_CFM_STOP = 2,
 	BT_6DOF_FLAGS_ERP_STOP = 4
 };
-#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
-
+#define BT_6DOF_FLAGS_AXIS_SHIFT 3  // bits per axis
 
 /// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
 /*!
@@ -276,254 +263,245 @@ This brings support for limit parameters and motors. </li>
 </ul>
 
 */
-ATTRIBUTE_ALIGNED16(class) btGeneric6DofConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class)
+btGeneric6DofConstraint : public btTypedConstraint
 {
 protected:
-
 	//! relative_frames
-    //!@{
-	btTransform	m_frameInA;//!< the constraint space w.r.t body A
-    btTransform	m_frameInB;//!< the constraint space w.r.t body B
-    //!@}
+	//!@{
+	btTransform m_frameInA;  //!< the constraint space w.r.t body A
+	btTransform m_frameInB;  //!< the constraint space w.r.t body B
+	//!@}
 
-    //! Jacobians
-    //!@{
-    btJacobianEntry	m_jacLinear[3];//!< 3 orthogonal linear constraints
-    btJacobianEntry	m_jacAng[3];//!< 3 orthogonal angular constraints
-    //!@}
+	//! Jacobians
+	//!@{
+	btJacobianEntry m_jacLinear[3];  //!< 3 orthogonal linear constraints
+	btJacobianEntry m_jacAng[3];     //!< 3 orthogonal angular constraints
+	//!@}
 
 	//! Linear_Limit_parameters
-    //!@{
-    btTranslationalLimitMotor m_linearLimits;
-    //!@}
-
-
-    //! hinge_parameters
-    //!@{
-    btRotationalLimitMotor m_angularLimits[3];
+	//!@{
+	btTranslationalLimitMotor m_linearLimits;
 	//!@}
 
+	//! hinge_parameters
+	//!@{
+	btRotationalLimitMotor m_angularLimits[3];
+	//!@}
 
 protected:
-    //! temporal variables
-    //!@{
-    btScalar m_timeStep;
-    btTransform m_calculatedTransformA;
-    btTransform m_calculatedTransformB;
-    btVector3 m_calculatedAxisAngleDiff;
-    btVector3 m_calculatedAxis[3];
-    btVector3 m_calculatedLinearDiff;
-	btScalar	m_factA;
-	btScalar	m_factB;
-	bool		m_hasStaticBody;
-    
-	btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
+	//! temporal variables
+	//!@{
+	btScalar m_timeStep;
+	btTransform m_calculatedTransformA;
+	btTransform m_calculatedTransformB;
+	btVector3 m_calculatedAxisAngleDiff;
+	btVector3 m_calculatedAxis[3];
+	btVector3 m_calculatedLinearDiff;
+	btScalar m_factA;
+	btScalar m_factB;
+	bool m_hasStaticBody;
 
-    bool	m_useLinearReferenceFrameA;
-	bool	m_useOffsetForConstraintFrame;
-    
-	int		m_flags;
+	btVector3 m_AnchorPos;  // point betwen pivots of bodies A and B to solve linear axes
 
-    //!@}
+	bool m_useLinearReferenceFrameA;
+	bool m_useOffsetForConstraintFrame;
 
-    btGeneric6DofConstraint&	operator=(btGeneric6DofConstraint&	other)
-    {
-        btAssert(0);
-        (void) other;
-        return *this;
-    }
+	int m_flags;
 
+	//!@}
 
-	int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+	btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other)
+	{
+		btAssert(0);
+		(void)other;
+		return *this;
+	}
 
-	int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+	int setAngularLimits(btConstraintInfo2 * info, int row_offset, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB);
 
-    void buildLinearJacobian(
-        btJacobianEntry & jacLinear,const btVector3 & normalWorld,
-        const btVector3 & pivotAInW,const btVector3 & pivotBInW);
+	int setLinearLimits(btConstraintInfo2 * info, int row, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB);
 
-    void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
+	void buildLinearJacobian(
+		btJacobianEntry & jacLinear, const btVector3& normalWorld,
+		const btVector3& pivotAInW, const btVector3& pivotBInW);
+
+	void buildAngularJacobian(btJacobianEntry & jacAngular, const btVector3& jointAxisW);
 
 	// tests linear limits
 	void calculateLinearInfo();
 
 	//! calcs the euler angles between the two bodies.
-    void calculateAngleInfo();
-
-
+	void calculateAngleInfo();
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
+
 	///for backwards compatibility during the transition to 'getInfo/getInfo2'
-	bool		m_useSolveConstraintObsolete;
+	bool m_useSolveConstraintObsolete;
+
+	btGeneric6DofConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA);
+	btGeneric6DofConstraint(btRigidBody & rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
 
-    btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
-    btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
-    
 	//! Calcs global transform of the offsets
 	/*!
 	Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
 	\sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
 	*/
-    void calculateTransforms(const btTransform& transA,const btTransform& transB);
+	void calculateTransforms(const btTransform& transA, const btTransform& transB);
 
 	void calculateTransforms();
 
 	//! Gets the global transform of the offset for body A
-    /*!
+	/*!
     \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
     */
-    const btTransform & getCalculatedTransformA() const
-    {
-    	return m_calculatedTransformA;
-    }
+	const btTransform& getCalculatedTransformA() const
+	{
+		return m_calculatedTransformA;
+	}
 
-    //! Gets the global transform of the offset for body B
-    /*!
+	//! Gets the global transform of the offset for body B
+	/*!
     \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
     */
-    const btTransform & getCalculatedTransformB() const
-    {
-    	return m_calculatedTransformB;
-    }
-
-    const btTransform & getFrameOffsetA() const
-    {
-    	return m_frameInA;
-    }
-
-    const btTransform & getFrameOffsetB() const
-    {
-    	return m_frameInB;
-    }
+	const btTransform& getCalculatedTransformB() const
+	{
+		return m_calculatedTransformB;
+	}
 
+	const btTransform& getFrameOffsetA() const
+	{
+		return m_frameInA;
+	}
 
-    btTransform & getFrameOffsetA()
-    {
-    	return m_frameInA;
-    }
+	const btTransform& getFrameOffsetB() const
+	{
+		return m_frameInB;
+	}
 
-    btTransform & getFrameOffsetB()
-    {
-    	return m_frameInB;
-    }
+	btTransform& getFrameOffsetA()
+	{
+		return m_frameInA;
+	}
 
+	btTransform& getFrameOffsetB()
+	{
+		return m_frameInB;
+	}
 
 	//! performs Jacobian calculation, and also calculates angle differences and axis
-    virtual void	buildJacobian();
-
-	virtual void getInfo1 (btConstraintInfo1* info);
+	virtual void buildJacobian();
 
-	void getInfo1NonVirtual (btConstraintInfo1* info);
+	virtual void getInfo1(btConstraintInfo1 * info);
 
-	virtual void getInfo2 (btConstraintInfo2* info);
+	void getInfo1NonVirtual(btConstraintInfo1 * info);
 
-	void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+	virtual void getInfo2(btConstraintInfo2 * info);
 
+	void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB);
 
-    void	updateRHS(btScalar	timeStep);
+	void updateRHS(btScalar timeStep);
 
 	//! Get the rotation axis in global coordinates
 	/*!
 	\pre btGeneric6DofConstraint.buildJacobian must be called previously.
 	*/
-    btVector3 getAxis(int axis_index) const;
+	btVector3 getAxis(int axis_index) const;
 
-    //! Get the relative Euler angle
-    /*!
+	//! Get the relative Euler angle
+	/*!
 	\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
 	*/
-    btScalar getAngle(int axis_index) const;
+	btScalar getAngle(int axis_index) const;
 
 	//! Get the relative position of the constraint pivot
-    /*!
+	/*!
 	\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
 	*/
 	btScalar getRelativePivotPosition(int axis_index) const;
 
-	void setFrames(const btTransform & frameA, const btTransform & frameB);
+	void setFrames(const btTransform& frameA, const btTransform& frameB);
 
 	//! Test angular limit.
 	/*!
 	Calculates angular correction and returns true if limit needs to be corrected.
 	\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
 	*/
-    bool testAngularLimitMotor(int axis_index);
+	bool testAngularLimitMotor(int axis_index);
 
-    void	setLinearLowerLimit(const btVector3& linearLower)
-    {
-    	m_linearLimits.m_lowerLimit = linearLower;
-    }
+	void setLinearLowerLimit(const btVector3& linearLower)
+	{
+		m_linearLimits.m_lowerLimit = linearLower;
+	}
 
-	void	getLinearLowerLimit(btVector3& linearLower) const
+	void getLinearLowerLimit(btVector3 & linearLower) const
 	{
 		linearLower = m_linearLimits.m_lowerLimit;
 	}
 
-	void	setLinearUpperLimit(const btVector3& linearUpper)
+	void setLinearUpperLimit(const btVector3& linearUpper)
 	{
 		m_linearLimits.m_upperLimit = linearUpper;
 	}
 
-	void	getLinearUpperLimit(btVector3& linearUpper) const
+	void getLinearUpperLimit(btVector3 & linearUpper) const
 	{
 		linearUpper = m_linearLimits.m_upperLimit;
 	}
 
-    void	setAngularLowerLimit(const btVector3& angularLower)
-    {
-		for(int i = 0; i < 3; i++) 
+	void setAngularLowerLimit(const btVector3& angularLower)
+	{
+		for (int i = 0; i < 3; i++)
 			m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
-    }
+	}
 
-	void	getAngularLowerLimit(btVector3& angularLower) const
+	void getAngularLowerLimit(btVector3 & angularLower) const
 	{
-		for(int i = 0; i < 3; i++) 
+		for (int i = 0; i < 3; i++)
 			angularLower[i] = m_angularLimits[i].m_loLimit;
 	}
 
-    void	setAngularUpperLimit(const btVector3& angularUpper)
-    {
-		for(int i = 0; i < 3; i++)
+	void setAngularUpperLimit(const btVector3& angularUpper)
+	{
+		for (int i = 0; i < 3; i++)
 			m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
-    }
+	}
 
-	void	getAngularUpperLimit(btVector3& angularUpper) const
+	void getAngularUpperLimit(btVector3 & angularUpper) const
 	{
-		for(int i = 0; i < 3; i++)
+		for (int i = 0; i < 3; i++)
 			angularUpper[i] = m_angularLimits[i].m_hiLimit;
 	}
 
 	//! Retrieves the angular limit informacion
-    btRotationalLimitMotor * getRotationalLimitMotor(int index)
-    {
-    	return &m_angularLimits[index];
-    }
-
-    //! Retrieves the  limit informacion
-    btTranslationalLimitMotor * getTranslationalLimitMotor()
-    {
-    	return &m_linearLimits;
-    }
-
-    //first 3 are linear, next 3 are angular
-    void setLimit(int axis, btScalar lo, btScalar hi)
-    {
-    	if(axis<3)
-    	{
-    		m_linearLimits.m_lowerLimit[axis] = lo;
-    		m_linearLimits.m_upperLimit[axis] = hi;
-    	}
-    	else
-    	{
+	btRotationalLimitMotor* getRotationalLimitMotor(int index)
+	{
+		return &m_angularLimits[index];
+	}
+
+	//! Retrieves the  limit informacion
+	btTranslationalLimitMotor* getTranslationalLimitMotor()
+	{
+		return &m_linearLimits;
+	}
+
+	//first 3 are linear, next 3 are angular
+	void setLimit(int axis, btScalar lo, btScalar hi)
+	{
+		if (axis < 3)
+		{
+			m_linearLimits.m_lowerLimit[axis] = lo;
+			m_linearLimits.m_upperLimit[axis] = hi;
+		}
+		else
+		{
 			lo = btNormalizeAngle(lo);
 			hi = btNormalizeAngle(hi);
-    		m_angularLimits[axis-3].m_loLimit = lo;
-    		m_angularLimits[axis-3].m_hiLimit = hi;
-    	}
-    }
+			m_angularLimits[axis - 3].m_loLimit = lo;
+			m_angularLimits[axis - 3].m_hiLimit = hi;
+		}
+	}
 
 	//! Test limit
 	/*!
@@ -532,116 +510,106 @@ public:
     - limited means upper > lower
     - limitIndex: first 3 are linear, next 3 are angular
     */
-    bool	isLimited(int limitIndex) const
-    {
-    	if(limitIndex<3)
-    	{
+	bool isLimited(int limitIndex) const
+	{
+		if (limitIndex < 3)
+		{
 			return m_linearLimits.isLimited(limitIndex);
+		}
+		return m_angularLimits[limitIndex - 3].isLimited();
+	}
 
-    	}
-        return m_angularLimits[limitIndex-3].isLimited();
-    }
-
-	virtual void calcAnchorPos(void); // overridable
+	virtual void calcAnchorPos(void);  // overridable
 
-	int get_limit_motor_info2(	btRotationalLimitMotor * limot,
-								const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
-								btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
+	int get_limit_motor_info2(btRotationalLimitMotor * limot,
+							  const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB,
+							  btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
 
 	// access for UseFrameOffset
 	bool getUseFrameOffset() const { return m_useOffsetForConstraintFrame; }
 	void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
-	
+
 	bool getUseLinearReferenceFrameA() const { return m_useLinearReferenceFrameA; }
 	void setUseLinearReferenceFrameA(bool linearReferenceFrameA) { m_useLinearReferenceFrameA = linearReferenceFrameA; }
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 	///If no axis is provided, it uses the default axis for this constraint.
-	virtual	void setParam(int num, btScalar value, int axis = -1);
+	virtual void setParam(int num, btScalar value, int axis = -1);
 	///return the local value of parameter
-	virtual	btScalar getParam(int num, int axis = -1) const;
+	virtual btScalar getParam(int num, int axis = -1) const;
 
-	void setAxis( const btVector3& axis1, const btVector3& axis2);
+	void setAxis(const btVector3& axis1, const btVector3& axis2);
 
-    	virtual	int getFlags() const
-    	{
-        	return m_flags;
+	virtual int getFlags() const
+	{
+		return m_flags;
 	}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	virtual int calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
-
-	
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
-
 struct btGeneric6DofConstraintData
 {
-	btTypedConstraintData	m_typeConstraintData;
-	btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTypedConstraintData m_typeConstraintData;
+	btTransformFloatData m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 	btTransformFloatData m_rbBFrame;
-	
-	btVector3FloatData	m_linearUpperLimit;
-	btVector3FloatData	m_linearLowerLimit;
-
-	btVector3FloatData	m_angularUpperLimit;
-	btVector3FloatData	m_angularLowerLimit;
-	
-	int	m_useLinearReferenceFrameA;
+
+	btVector3FloatData m_linearUpperLimit;
+	btVector3FloatData m_linearLowerLimit;
+
+	btVector3FloatData m_angularUpperLimit;
+	btVector3FloatData m_angularLowerLimit;
+
+	int m_useLinearReferenceFrameA;
 	int m_useOffsetForConstraintFrame;
 };
 
 struct btGeneric6DofConstraintDoubleData2
 {
-	btTypedConstraintDoubleData	m_typeConstraintData;
-	btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTypedConstraintDoubleData m_typeConstraintData;
+	btTransformDoubleData m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 	btTransformDoubleData m_rbBFrame;
-	
-	btVector3DoubleData	m_linearUpperLimit;
-	btVector3DoubleData	m_linearLowerLimit;
-
-	btVector3DoubleData	m_angularUpperLimit;
-	btVector3DoubleData	m_angularLowerLimit;
-	
-	int	m_useLinearReferenceFrameA;
+
+	btVector3DoubleData m_linearUpperLimit;
+	btVector3DoubleData m_linearLowerLimit;
+
+	btVector3DoubleData m_angularUpperLimit;
+	btVector3DoubleData m_angularLowerLimit;
+
+	int m_useLinearReferenceFrameA;
 	int m_useOffsetForConstraintFrame;
 };
 
-SIMD_FORCE_INLINE	int	btGeneric6DofConstraint::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(btGeneric6DofConstraintData2);
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-
 	btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer;
-	btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
+	btTypedConstraint::serialize(&dof->m_typeConstraintData, serializer);
 
 	m_frameInA.serialize(dof->m_rbAFrame);
 	m_frameInB.serialize(dof->m_rbBFrame);
 
-		
 	int i;
-	for (i=0;i<3;i++)
+	for (i = 0; i < 3; i++)
 	{
-		dof->m_angularLowerLimit.m_floats[i] =  m_angularLimits[i].m_loLimit;
-		dof->m_angularUpperLimit.m_floats[i] =  m_angularLimits[i].m_hiLimit;
+		dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit;
+		dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit;
 		dof->m_linearLowerLimit.m_floats[i] = m_linearLimits.m_lowerLimit[i];
 		dof->m_linearUpperLimit.m_floats[i] = m_linearLimits.m_upperLimit[i];
 	}
-	
-	dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
+
+	dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA ? 1 : 0;
 	dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
 
 	return btGeneric6DofConstraintDataName;
 }
 
-
-
-
-
-#endif //BT_GENERIC_6DOF_CONSTRAINT_H
+#endif  //BT_GENERIC_6DOF_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
index 31371944864..74a13c62493 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
@@ -37,67 +37,55 @@ email: projectileman@yahoo.com
 http://gimpact.sf.net
 */
 
-
-
 #include "btGeneric6DofSpring2Constraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
+#include <cmath>
 #include <new>
 
-
-
 btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder)
-	: btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB)
-	, m_frameInA(frameInA)
-	, m_frameInB(frameInB)
-	, m_rotateOrder(rotOrder)	
-	, m_flags(0)
+	: btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_rotateOrder(rotOrder), m_flags(0)
 {
 	calculateTransforms();
 }
 
-
 btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder)
-	: btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB)
-	, m_frameInB(frameInB)
-	, m_rotateOrder(rotOrder)
-	, m_flags(0)
+	: btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB), m_frameInB(frameInB), m_rotateOrder(rotOrder), m_flags(0)
 {
 	///not providing rigidbody A means implicitly using worldspace for body A
 	m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
 	calculateTransforms();
 }
 
-
 btScalar btGeneric6DofSpring2Constraint::btGetMatrixElem(const btMatrix3x3& mat, int index)
 {
-	int i = index%3;
-	int j = index/3;
+	int i = index % 3;
+	int j = index / 3;
 	return mat[i][j];
 }
 
 // MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
 
-bool btGeneric6DofSpring2Constraint::matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
+bool btGeneric6DofSpring2Constraint::matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz)
 {
 	// rot =  cy*cz          -cy*sz           sy
 	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
 	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
 
-	btScalar fi = btGetMatrixElem(mat,2);
+	btScalar fi = btGetMatrixElem(mat, 2);
 	if (fi < btScalar(1.0f))
 	{
 		if (fi > btScalar(-1.0f))
 		{
-			xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
-			xyz[1] = btAsin(btGetMatrixElem(mat,2));
-			xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+			xyz[0] = btAtan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
+			xyz[1] = btAsin(btGetMatrixElem(mat, 2));
+			xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
 			return true;
 		}
 		else
 		{
 			// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
-			xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+			xyz[0] = -btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
 			xyz[1] = -SIMD_HALF_PI;
 			xyz[2] = btScalar(0.0);
 			return false;
@@ -106,32 +94,32 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerXYZ(const btMatrix3x3& mat,btV
 	else
 	{
 		// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
-		xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+		xyz[0] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
 		xyz[1] = SIMD_HALF_PI;
 		xyz[2] = 0.0;
 	}
 	return false;
 }
 
-bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz)
+bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat, btVector3& xyz)
 {
 	// rot =  cy*cz          -sz           sy*cz
 	//        cy*cx*sz+sx*sy  cx*cz        sy*cx*sz-cy*sx
 	//        cy*sx*sz-cx*sy  sx*cz        sy*sx*sz+cx*cy
 
-	btScalar fi = btGetMatrixElem(mat,1);
+	btScalar fi = btGetMatrixElem(mat, 1);
 	if (fi < btScalar(1.0f))
 	{
 		if (fi > btScalar(-1.0f))
 		{
-			xyz[0] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,4));
-			xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
-			xyz[2] = btAsin(-btGetMatrixElem(mat,1));
+			xyz[0] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 4));
+			xyz[1] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
+			xyz[2] = btAsin(-btGetMatrixElem(mat, 1));
 			return true;
 		}
 		else
 		{
-			xyz[0] = -btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
+			xyz[0] = -btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
 			xyz[1] = btScalar(0.0);
 			xyz[2] = SIMD_HALF_PI;
 			return false;
@@ -139,33 +127,33 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat,btV
 	}
 	else
 	{
-		xyz[0] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
+		xyz[0] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
 		xyz[1] = 0.0;
 		xyz[2] = -SIMD_HALF_PI;
 	}
 	return false;
 }
 
-bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz)
+bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat, btVector3& xyz)
 {
 	// rot =  cy*cz+sy*sx*sz  cz*sy*sx-cy*sz  cx*sy
 	//        cx*sz           cx*cz           -sx
 	//        cy*sx*sz-cz*sy  sy*sz+cy*cz*sx  cy*cx
 
-	btScalar fi = btGetMatrixElem(mat,5);
+	btScalar fi = btGetMatrixElem(mat, 5);
 	if (fi < btScalar(1.0f))
 	{
 		if (fi > btScalar(-1.0f))
 		{
-			xyz[0] = btAsin(-btGetMatrixElem(mat,5));
-			xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,8));
-			xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+			xyz[0] = btAsin(-btGetMatrixElem(mat, 5));
+			xyz[1] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 8));
+			xyz[2] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
 			return true;
 		}
 		else
 		{
 			xyz[0] = SIMD_HALF_PI;
-			xyz[1] = -btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+			xyz[1] = -btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
 			xyz[2] = btScalar(0.0);
 			return false;
 		}
@@ -173,32 +161,32 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat,btV
 	else
 	{
 		xyz[0] = -SIMD_HALF_PI;
-		xyz[1] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+		xyz[1] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
 		xyz[2] = 0.0;
 	}
 	return false;
 }
 
-bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz)
+bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat, btVector3& xyz)
 {
 	// rot =  cy*cz   sy*sx-cy*cx*sz   cx*sy+cy*sz*sx
 	//        sz           cz*cx           -cz*sx
 	//        -cz*sy  cy*sx+cx*sy*sz   cy*cx-sy*sz*sx
 
-	btScalar fi = btGetMatrixElem(mat,3);
+	btScalar fi = btGetMatrixElem(mat, 3);
 	if (fi < btScalar(1.0f))
 	{
 		if (fi > btScalar(-1.0f))
 		{
-			xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,4));
-			xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,0));
-			xyz[2] = btAsin(btGetMatrixElem(mat,3));
+			xyz[0] = btAtan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 4));
+			xyz[1] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 0));
+			xyz[2] = btAsin(btGetMatrixElem(mat, 3));
 			return true;
 		}
 		else
 		{
 			xyz[0] = btScalar(0.0);
-			xyz[1] = -btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8));
+			xyz[1] = -btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
 			xyz[2] = -SIMD_HALF_PI;
 			return false;
 		}
@@ -206,33 +194,33 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btV
 	else
 	{
 		xyz[0] = btScalar(0.0);
-		xyz[1] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8));
+		xyz[1] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
 		xyz[2] = SIMD_HALF_PI;
 	}
 	return false;
 }
 
-bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz)
+bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat, btVector3& xyz)
 {
 	// rot =  cz*cy-sz*sx*sy    -cx*sz   cz*sy+cy*sz*sx
 	//        cy*sz+cz*sx*sy     cz*cx   sz*sy-cz*xy*sx
 	//        -cx*sy              sx     cx*cy
 
-	btScalar fi = btGetMatrixElem(mat,7);
+	btScalar fi = btGetMatrixElem(mat, 7);
 	if (fi < btScalar(1.0f))
 	{
 		if (fi > btScalar(-1.0f))
 		{
-			xyz[0] = btAsin(btGetMatrixElem(mat,7));
-			xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
-			xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,4));
+			xyz[0] = btAsin(btGetMatrixElem(mat, 7));
+			xyz[1] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
+			xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 4));
 			return true;
 		}
 		else
 		{
 			xyz[0] = -SIMD_HALF_PI;
 			xyz[1] = btScalar(0.0);
-			xyz[2] = -btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
+			xyz[2] = -btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
 			return false;
 		}
 	}
@@ -240,32 +228,32 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btV
 	{
 		xyz[0] = SIMD_HALF_PI;
 		xyz[1] = btScalar(0.0);
-		xyz[2] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
+		xyz[2] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
 	}
 	return false;
 }
 
-bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz)
+bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat, btVector3& xyz)
 {
 	// rot =  cz*cy   cz*sy*sx-cx*sz   sz*sx+cz*cx*sy
 	//        cy*sz   cz*cx+sz*sy*sx   cx*sz*sy-cz*sx
 	//        -sy          cy*sx         cy*cx
 
-	btScalar fi = btGetMatrixElem(mat,6);
+	btScalar fi = btGetMatrixElem(mat, 6);
 	if (fi < btScalar(1.0f))
 	{
 		if (fi > btScalar(-1.0f))
 		{
-			xyz[0] = btAtan2(btGetMatrixElem(mat,7), btGetMatrixElem(mat,8));
-			xyz[1] = btAsin(-btGetMatrixElem(mat,6));
-			xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,0));
+			xyz[0] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
+			xyz[1] = btAsin(-btGetMatrixElem(mat, 6));
+			xyz[2] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 0));
 			return true;
 		}
 		else
 		{
 			xyz[0] = btScalar(0.0);
 			xyz[1] = SIMD_HALF_PI;
-			xyz[2] = -btAtan2(btGetMatrixElem(mat,1),btGetMatrixElem(mat,2));
+			xyz[2] = -btAtan2(btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 2));
 			return false;
 		}
 	}
@@ -273,23 +261,36 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btV
 	{
 		xyz[0] = btScalar(0.0);
 		xyz[1] = -SIMD_HALF_PI;
-		xyz[2] = btAtan2(-btGetMatrixElem(mat,1),-btGetMatrixElem(mat,2));
+		xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), -btGetMatrixElem(mat, 2));
 	}
 	return false;
 }
 
 void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 {
-	btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
+	btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis();
 	switch (m_rotateOrder)
 	{
-		case RO_XYZ : matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); break;
-		case RO_XZY : matrixToEulerXZY(relative_frame,m_calculatedAxisAngleDiff); break;
-		case RO_YXZ : matrixToEulerYXZ(relative_frame,m_calculatedAxisAngleDiff); break;
-		case RO_YZX : matrixToEulerYZX(relative_frame,m_calculatedAxisAngleDiff); break;
-		case RO_ZXY : matrixToEulerZXY(relative_frame,m_calculatedAxisAngleDiff); break;
-		case RO_ZYX : matrixToEulerZYX(relative_frame,m_calculatedAxisAngleDiff); break;
-		default : btAssert(false);
+		case RO_XYZ:
+			matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff);
+			break;
+		case RO_XZY:
+			matrixToEulerXZY(relative_frame, m_calculatedAxisAngleDiff);
+			break;
+		case RO_YXZ:
+			matrixToEulerYXZ(relative_frame, m_calculatedAxisAngleDiff);
+			break;
+		case RO_YZX:
+			matrixToEulerYZX(relative_frame, m_calculatedAxisAngleDiff);
+			break;
+		case RO_ZXY:
+			matrixToEulerZXY(relative_frame, m_calculatedAxisAngleDiff);
+			break;
+		case RO_ZYX:
+			matrixToEulerZYX(relative_frame, m_calculatedAxisAngleDiff);
+			break;
+		default:
+			btAssert(false);
 	}
 	// in euler angle mode we do not actually constrain the angular velocity
 	// along the axes axis[0] and axis[2] (although we do use axis[1]) :
@@ -307,14 +308,14 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 	// to the components of w and set that to 0.
 	switch (m_rotateOrder)
 	{
-	case RO_XYZ :
+		case RO_XYZ:
 		{
 			//Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles)
-			//The two planes are non-homologous, so this is a Tait–Bryan angle formalism and not a proper Euler
+			//The two planes are non-homologous, so this is a Tait Bryan angle formalism and not a proper Euler
 			//Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation)
-			//that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait–Bryan angles)
+			//that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait Bryan angles)
 			// x' = Nperp = N.cross(axis2)
-			// y' = N = axis2.cross(axis0)	
+			// y' = N = axis2.cross(axis0)
 			// z' = z
 			//
 			// x" = X
@@ -324,7 +325,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 			//first rotate around z
 			//second rotate around y'= z.cross(X)
 			//third rotate around x" = X
-			//Original XYZ extrinsic rotation order. 
+			//Original XYZ extrinsic rotation order.
 			//Planes: xy and YZ normals: z, X.  Plane intersection (N) is z.cross(X)
 			btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
 			btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
@@ -333,7 +334,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 			m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
 			break;
 		}
-	case RO_XZY :
+		case RO_XZY:
 		{
 			//planes: xz,ZY normals: y, X
 			//first rotate around y
@@ -346,7 +347,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 			m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
 			break;
 		}
-	case RO_YXZ :
+		case RO_YXZ:
 		{
 			//planes: yx,XZ normals: z, Y
 			//first rotate around z
@@ -359,7 +360,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 			m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
 			break;
 		}
-	case RO_YZX :
+		case RO_YZX:
 		{
 			//planes: yz,ZX normals: x, Y
 			//first rotate around x
@@ -372,7 +373,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 			m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
 			break;
 		}
-	case RO_ZXY :
+		case RO_ZXY:
 		{
 			//planes: zx,XY normals: y, Z
 			//first rotate around y
@@ -385,7 +386,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 			m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
 			break;
 		}
-	case RO_ZYX :
+		case RO_ZYX:
 		{
 			//planes: zy,YX normals: x, Z
 			//first rotate around x
@@ -398,22 +399,21 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
 			m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
 			break;
 		}
-	default:
-		btAssert(false);
+		default:
+			btAssert(false);
 	}
 
 	m_calculatedAxis[0].normalize();
 	m_calculatedAxis[1].normalize();
 	m_calculatedAxis[2].normalize();
-
 }
 
 void btGeneric6DofSpring2Constraint::calculateTransforms()
 {
-	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 }
 
-void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA, const btTransform& transB)
 {
 	m_calculatedTransformA = transA * m_frameInA;
 	m_calculatedTransformB = transB * m_frameInB;
@@ -424,18 +424,17 @@ void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& tran
 	btScalar miB = getRigidBodyB().getInvMass();
 	m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
 	btScalar miS = miA + miB;
-	if(miS > btScalar(0.f))
+	if (miS > btScalar(0.f))
 	{
 		m_factA = miB / miS;
 	}
-	else 
+	else
 	{
 		m_factA = btScalar(0.5f);
 	}
 	m_factB = btScalar(1.0f) - m_factA;
 }
 
-
 void btGeneric6DofSpring2Constraint::testAngularLimitMotor(int axis_index)
 {
 	btScalar angle = m_calculatedAxisAngleDiff[axis_index];
@@ -444,35 +443,37 @@ void btGeneric6DofSpring2Constraint::testAngularLimitMotor(int axis_index)
 	m_angularLimits[axis_index].testLimitValue(angle);
 }
 
-
-void btGeneric6DofSpring2Constraint::getInfo1 (btConstraintInfo1* info)
+void btGeneric6DofSpring2Constraint::getInfo1(btConstraintInfo1* info)
 {
 	//prepare constraint
-	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 	info->m_numConstraintRows = 0;
 	info->nub = 0;
 	int i;
 	//test linear limits
-	for(i = 0; i < 3; i++)
+	for (i = 0; i < 3; i++)
 	{
-		     if (m_linearLimits.m_currentLimit[i]==4) info->m_numConstraintRows += 2;
-		else if (m_linearLimits.m_currentLimit[i]!=0) info->m_numConstraintRows += 1;
-		if (m_linearLimits.m_enableMotor[i] ) info->m_numConstraintRows += 1;
+		if (m_linearLimits.m_currentLimit[i] == 4)
+			info->m_numConstraintRows += 2;
+		else if (m_linearLimits.m_currentLimit[i] != 0)
+			info->m_numConstraintRows += 1;
+		if (m_linearLimits.m_enableMotor[i]) info->m_numConstraintRows += 1;
 		if (m_linearLimits.m_enableSpring[i]) info->m_numConstraintRows += 1;
 	}
 	//test angular limits
-	for (i=0;i<3 ;i++ )
+	for (i = 0; i < 3; i++)
 	{
 		testAngularLimitMotor(i);
-		     if (m_angularLimits[i].m_currentLimit==4) info->m_numConstraintRows += 2;
-		else if (m_angularLimits[i].m_currentLimit!=0) info->m_numConstraintRows += 1;
-		if (m_angularLimits[i].m_enableMotor ) info->m_numConstraintRows += 1;
+		if (m_angularLimits[i].m_currentLimit == 4)
+			info->m_numConstraintRows += 2;
+		else if (m_angularLimits[i].m_currentLimit != 0)
+			info->m_numConstraintRows += 1;
+		if (m_angularLimits[i].m_enableMotor) info->m_numConstraintRows += 1;
 		if (m_angularLimits[i].m_enableSpring) info->m_numConstraintRows += 1;
 	}
 }
 
-
-void btGeneric6DofSpring2Constraint::getInfo2 (btConstraintInfo2* info)
+void btGeneric6DofSpring2Constraint::getInfo2(btConstraintInfo2* info)
 {
 	const btTransform& transA = m_rbA.getCenterOfMassTransform();
 	const btTransform& transB = m_rbB.getCenterOfMassTransform();
@@ -482,118 +483,138 @@ void btGeneric6DofSpring2Constraint::getInfo2 (btConstraintInfo2* info)
 	const btVector3& angVelB = m_rbB.getAngularVelocity();
 
 	// for stability better to solve angular limits first
-	int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
-	setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
+	int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+	setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 }
 
-
-int btGeneric6DofSpring2Constraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+int btGeneric6DofSpring2Constraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
 {
 	//solve linear limits
 	btRotationalLimitMotor2 limot;
-	for (int i=0;i<3 ;i++ )
+	for (int i = 0; i < 3; i++)
 	{
-		if(m_linearLimits.m_currentLimit[i] || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i])
-		{ // re-use rotational motor code
-			limot.m_bounce                 = m_linearLimits.m_bounce[i];
-			limot.m_currentLimit           = m_linearLimits.m_currentLimit[i];
-			limot.m_currentPosition        = m_linearLimits.m_currentLinearDiff[i];
-			limot.m_currentLimitError      = m_linearLimits.m_currentLimitError[i];
-			limot.m_currentLimitErrorHi    = m_linearLimits.m_currentLimitErrorHi[i];
-			limot.m_enableMotor            = m_linearLimits.m_enableMotor[i];
-			limot.m_servoMotor             = m_linearLimits.m_servoMotor[i];
-			limot.m_servoTarget            = m_linearLimits.m_servoTarget[i];
-			limot.m_enableSpring           = m_linearLimits.m_enableSpring[i];
-			limot.m_springStiffness        = m_linearLimits.m_springStiffness[i];
+		if (m_linearLimits.m_currentLimit[i] || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i])
+		{  // re-use rotational motor code
+			limot.m_bounce = m_linearLimits.m_bounce[i];
+			limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
+			limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
+			limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
+			limot.m_currentLimitErrorHi = m_linearLimits.m_currentLimitErrorHi[i];
+			limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
+			limot.m_servoMotor = m_linearLimits.m_servoMotor[i];
+			limot.m_servoTarget = m_linearLimits.m_servoTarget[i];
+			limot.m_enableSpring = m_linearLimits.m_enableSpring[i];
+			limot.m_springStiffness = m_linearLimits.m_springStiffness[i];
 			limot.m_springStiffnessLimited = m_linearLimits.m_springStiffnessLimited[i];
-			limot.m_springDamping          = m_linearLimits.m_springDamping[i];
-			limot.m_springDampingLimited   = m_linearLimits.m_springDampingLimited[i];
-			limot.m_equilibriumPoint       = m_linearLimits.m_equilibriumPoint[i];
-			limot.m_hiLimit                = m_linearLimits.m_upperLimit[i];
-			limot.m_loLimit                = m_linearLimits.m_lowerLimit[i];
-			limot.m_maxMotorForce          = m_linearLimits.m_maxMotorForce[i];
-			limot.m_targetVelocity         = m_linearLimits.m_targetVelocity[i];
+			limot.m_springDamping = m_linearLimits.m_springDamping[i];
+			limot.m_springDampingLimited = m_linearLimits.m_springDampingLimited[i];
+			limot.m_equilibriumPoint = m_linearLimits.m_equilibriumPoint[i];
+			limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
+			limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
+			limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
+			limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
 			btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
 			int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT2);
-			limot.m_stopCFM  = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
-			limot.m_stopERP  = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp;
+			limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
+			limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp;
 			limot.m_motorCFM = (flags & BT_6DOF_FLAGS_CFM_MOTO2) ? m_linearLimits.m_motorCFM[i] : info->cfm[0];
 			limot.m_motorERP = (flags & BT_6DOF_FLAGS_ERP_MOTO2) ? m_linearLimits.m_motorERP[i] : info->erp;
 
 			//rotAllowed is a bit of a magic from the original 6dof. The calculation of it here is something that imitates the original behavior as much as possible.
 			int indx1 = (i + 1) % 3;
 			int indx2 = (i + 2) % 3;
-			int rotAllowed = 1; // rotations around orthos to current axis (it is used only when one of the body is static)
-			#define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3
+			int rotAllowed = 1;  // rotations around orthos to current axis (it is used only when one of the body is static)
+#define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3
 			bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 ||
-				m_angularLimits[indx1].m_currentLimit == 2 ||
-				( m_angularLimits[indx1].m_currentLimit == 3 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) ||
-				( m_angularLimits[indx1].m_currentLimit == 4 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) );
+								 m_angularLimits[indx1].m_currentLimit == 2 ||
+								 (m_angularLimits[indx1].m_currentLimit == 3 && (m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION)) ||
+								 (m_angularLimits[indx1].m_currentLimit == 4 && (m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION));
 			bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 ||
-				m_angularLimits[indx2].m_currentLimit == 2 ||
-				( m_angularLimits[indx2].m_currentLimit == 3 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) ||
-				( m_angularLimits[indx2].m_currentLimit == 4 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) );
-			if( indx1Violated && indx2Violated )
+								 m_angularLimits[indx2].m_currentLimit == 2 ||
+								 (m_angularLimits[indx2].m_currentLimit == 3 && (m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION)) ||
+								 (m_angularLimits[indx2].m_currentLimit == 4 && (m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION));
+			if (indx1Violated && indx2Violated)
 			{
 				rotAllowed = 0;
 			}
-			row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
-
+			row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
 		}
 	}
 	return row;
 }
 
-
-
-int btGeneric6DofSpring2Constraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+int btGeneric6DofSpring2Constraint::setAngularLimits(btConstraintInfo2* info, int row_offset, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
 {
 	int row = row_offset;
 
 	//order of rotational constraint rows
 	int cIdx[] = {0, 1, 2};
-	switch(m_rotateOrder)
+	switch (m_rotateOrder)
 	{
-		case RO_XYZ : cIdx[0] = 0; cIdx[1] = 1; cIdx[2] = 2; break;
-		case RO_XZY : cIdx[0] = 0; cIdx[1] = 2; cIdx[2] = 1; break;
-		case RO_YXZ : cIdx[0] = 1; cIdx[1] = 0; cIdx[2] = 2; break;
-		case RO_YZX : cIdx[0] = 1; cIdx[1] = 2; cIdx[2] = 0; break;
-		case RO_ZXY : cIdx[0] = 2; cIdx[1] = 0; cIdx[2] = 1; break;
-		case RO_ZYX : cIdx[0] = 2; cIdx[1] = 1; cIdx[2] = 0; break;
-		default : btAssert(false);
+		case RO_XYZ:
+			cIdx[0] = 0;
+			cIdx[1] = 1;
+			cIdx[2] = 2;
+			break;
+		case RO_XZY:
+			cIdx[0] = 0;
+			cIdx[1] = 2;
+			cIdx[2] = 1;
+			break;
+		case RO_YXZ:
+			cIdx[0] = 1;
+			cIdx[1] = 0;
+			cIdx[2] = 2;
+			break;
+		case RO_YZX:
+			cIdx[0] = 1;
+			cIdx[1] = 2;
+			cIdx[2] = 0;
+			break;
+		case RO_ZXY:
+			cIdx[0] = 2;
+			cIdx[1] = 0;
+			cIdx[2] = 1;
+			break;
+		case RO_ZYX:
+			cIdx[0] = 2;
+			cIdx[1] = 1;
+			cIdx[2] = 0;
+			break;
+		default:
+			btAssert(false);
 	}
 
-	for (int ii = 0; ii < 3 ; ii++ )
+	for (int ii = 0; ii < 3; ii++)
 	{
 		int i = cIdx[ii];
-		if(m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring)
+		if (m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring)
 		{
 			btVector3 axis = getAxis(i);
 			int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT2);
-			if(!(flags & BT_6DOF_FLAGS_CFM_STOP2))
+			if (!(flags & BT_6DOF_FLAGS_CFM_STOP2))
 			{
 				m_angularLimits[i].m_stopCFM = info->cfm[0];
 			}
-			if(!(flags & BT_6DOF_FLAGS_ERP_STOP2))
+			if (!(flags & BT_6DOF_FLAGS_ERP_STOP2))
 			{
 				m_angularLimits[i].m_stopERP = info->erp;
 			}
-			if(!(flags & BT_6DOF_FLAGS_CFM_MOTO2))
+			if (!(flags & BT_6DOF_FLAGS_CFM_MOTO2))
 			{
 				m_angularLimits[i].m_motorCFM = info->cfm[0];
 			}
-			if(!(flags & BT_6DOF_FLAGS_ERP_MOTO2))
+			if (!(flags & BT_6DOF_FLAGS_ERP_MOTO2))
 			{
 				m_angularLimits[i].m_motorERP = info->erp;
 			}
-			row += get_limit_motor_info2(&m_angularLimits[i],transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
+			row += get_limit_motor_info2(&m_angularLimits[i], transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
 		}
 	}
 
 	return row;
 }
 
-
 void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const btTransform& frameB)
 {
 	m_frameInA = frameA;
@@ -602,32 +623,31 @@ void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const
 	calculateTransforms();
 }
 
-
 void btGeneric6DofSpring2Constraint::calculateLinearInfo()
 {
 	m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
 	m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
-	for(int i = 0; i < 3; i++)
+	for (int i = 0; i < 3; i++)
 	{
 		m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
 		m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
 	}
 }
 
-void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2 * limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2 *info, int srow, btVector3& ax1, int rotational, int rotAllowed)
+void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA, const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed)
 {
-	btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
-	btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
+	btScalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
+	btScalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
 
-	J1[srow+0] = ax1[0];
-	J1[srow+1] = ax1[1];
-	J1[srow+2] = ax1[2];
+	J1[srow + 0] = ax1[0];
+	J1[srow + 1] = ax1[1];
+	J1[srow + 2] = ax1[2];
 
-	J2[srow+0] = -ax1[0];
-	J2[srow+1] = -ax1[1];
-	J2[srow+2] = -ax1[2];
+	J2[srow + 0] = -ax1[0];
+	J2[srow + 1] = -ax1[1];
+	J2[srow + 2] = -ax1[2];
 
-	if(!rotational)
+	if (!rotational)
 	{
 		btVector3 tmpA, tmpB, relA, relB;
 		// get vector from bodyB to frameB in WCS
@@ -636,40 +656,44 @@ void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2 * l
 		relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
 		tmpA = relA.cross(ax1);
 		tmpB = relB.cross(ax1);
-		if(m_hasStaticBody && (!rotAllowed))
+		if (m_hasStaticBody && (!rotAllowed))
 		{
 			tmpA *= m_factA;
 			tmpB *= m_factB;
 		}
 		int i;
-		for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
-		for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
+		for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
+		for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
 	}
 }
 
-
 int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
-	btRotationalLimitMotor2 * limot,
-	const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
-	btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
+	btRotationalLimitMotor2* limot,
+	const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB,
+	btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed)
 {
 	int count = 0;
 	int srow = row * info->rowskip;
 
-	if (limot->m_currentLimit==4) 
+	if (limot->m_currentLimit == 4)
 	{
 		btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
 
-		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
 		info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
-		if (rotational) {
-			if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) {
-				btScalar bounceerror = -limot->m_bounce* vel;
+		if (rotational)
+		{
+			if (info->m_constraintError[srow] - vel * limot->m_stopERP > 0)
+			{
+				btScalar bounceerror = -limot->m_bounce * vel;
 				if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
 			}
-		} else {
-			if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) {
-				btScalar bounceerror = -limot->m_bounce* vel;
+		}
+		else
+		{
+			if (info->m_constraintError[srow] - vel * limot->m_stopERP < 0)
+			{
+				btScalar bounceerror = -limot->m_bounce * vel;
 				if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
 			}
 		}
@@ -679,16 +703,21 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		srow += info->rowskip;
 		++count;
 
-		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
 		info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitErrorHi * (rotational ? -1 : 1);
-		if (rotational) {
-			if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) {
-				btScalar bounceerror = -limot->m_bounce* vel;
+		if (rotational)
+		{
+			if (info->m_constraintError[srow] - vel * limot->m_stopERP < 0)
+			{
+				btScalar bounceerror = -limot->m_bounce * vel;
 				if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
 			}
-		} else {
-			if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) {
-				btScalar bounceerror = -limot->m_bounce* vel;
+		}
+		else
+		{
+			if (info->m_constraintError[srow] - vel * limot->m_stopERP > 0)
+			{
+				btScalar bounceerror = -limot->m_bounce * vel;
 				if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
 			}
 		}
@@ -697,10 +726,10 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		info->cfm[srow] = limot->m_stopCFM;
 		srow += info->rowskip;
 		++count;
-	} else
-	if (limot->m_currentLimit==3) 
+	}
+	else if (limot->m_currentLimit == 3)
 	{
-		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
 		info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
 		info->m_lowerLimit[srow] = -SIMD_INFINITY;
 		info->m_upperLimit[srow] = SIMD_INFINITY;
@@ -711,16 +740,16 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 
 	if (limot->m_enableMotor && !limot->m_servoMotor)
 	{
-		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
 		btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
-		btScalar mot_fact = getMotorFactor(limot->m_currentPosition, 
-			limot->m_loLimit,
-			limot->m_hiLimit,
-			tag_vel,
-			info->fps * limot->m_motorERP);
+		btScalar mot_fact = getMotorFactor(limot->m_currentPosition,
+										   limot->m_loLimit,
+										   limot->m_hiLimit,
+										   tag_vel,
+										   info->fps * limot->m_motorERP);
 		info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity;
-		info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
-		info->m_upperLimit[srow] = limot->m_maxMotorForce;
+		info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
+		info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
 		info->cfm[srow] = limot->m_motorCFM;
 		srow += info->rowskip;
 		++count;
@@ -729,33 +758,48 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 	if (limot->m_enableMotor && limot->m_servoMotor)
 	{
 		btScalar error = limot->m_currentPosition - limot->m_servoTarget;
-		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
-		btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
+		btScalar curServoTarget = limot->m_servoTarget;
+		if (rotational)
+		{
+			if (error > SIMD_PI)
+			{
+				error -= SIMD_2_PI;
+				curServoTarget += SIMD_2_PI;
+			}
+			if (error < -SIMD_PI)
+			{
+				error += SIMD_2_PI;
+				curServoTarget -= SIMD_2_PI;
+			}
+		}
+
+		calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
+		btScalar targetvelocity = error < 0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
 		btScalar tag_vel = -targetvelocity;
 		btScalar mot_fact;
-		if(error != 0)
+		if (error != 0)
 		{
 			btScalar lowLimit;
 			btScalar hiLimit;
-			if(limot->m_loLimit > limot->m_hiLimit)
+			if (limot->m_loLimit > limot->m_hiLimit)
 			{
-				lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY;
-				hiLimit  = error < 0 ? limot->m_servoTarget :  SIMD_INFINITY;
+				lowLimit = error > 0 ? curServoTarget : -SIMD_INFINITY;
+				hiLimit = error < 0 ? curServoTarget : SIMD_INFINITY;
 			}
 			else
 			{
-				lowLimit = error > 0 && limot->m_servoTarget>limot->m_loLimit ? limot->m_servoTarget : limot->m_loLimit;
-				hiLimit  = error < 0 && limot->m_servoTarget<limot->m_hiLimit ? limot->m_servoTarget : limot->m_hiLimit;
+				lowLimit = error > 0 && curServoTarget > limot->m_loLimit ? curServoTarget : limot->m_loLimit;
+				hiLimit = error < 0 && curServoTarget < limot->m_hiLimit ? curServoTarget : limot->m_hiLimit;
 			}
 			mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
-		} 
-		else 
+		}
+		else
 		{
 			mot_fact = 0;
 		}
 		info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1);
-		info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
-		info->m_upperLimit[srow] = limot->m_maxMotorForce;
+		info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
+		info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
 		info->cfm[srow] = limot->m_motorCFM;
 		srow += info->rowskip;
 		++count;
@@ -764,7 +808,7 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 	if (limot->m_enableSpring)
 	{
 		btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint;
-		calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+		calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
 
 		//btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping);
 		//if(cfm > 0.99999)
@@ -777,51 +821,69 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		btScalar dt = BT_ONE / info->fps;
 		btScalar kd = limot->m_springDamping;
 		btScalar ks = limot->m_springStiffness;
-		btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
-//		btScalar erp = 0.1;
+		btScalar vel;
+		if (rotational)
+		{
+			vel = angVelA.dot(ax1) - angVelB.dot(ax1);
+		}
+		else
+		{
+			btVector3 tanVelA = angVelA.cross(m_calculatedTransformA.getOrigin() - transA.getOrigin());
+			btVector3 tanVelB = angVelB.cross(m_calculatedTransformB.getOrigin() - transB.getOrigin());
+			vel = (linVelA + tanVelA).dot(ax1) - (linVelB + tanVelB).dot(ax1);
+		}
 		btScalar cfm = BT_ZERO;
 		btScalar mA = BT_ONE / m_rbA.getInvMass();
 		btScalar mB = BT_ONE / m_rbB.getInvMass();
-		if (rotational) {
+		if (rotational)
+		{
 			btScalar rrA = (m_calculatedTransformA.getOrigin() - transA.getOrigin()).length2();
 			btScalar rrB = (m_calculatedTransformB.getOrigin() - transB.getOrigin()).length2();
 			if (m_rbA.getInvMass()) mA = mA * rrA + 1 / (m_rbA.getInvInertiaTensorWorld() * ax1).length();
 			if (m_rbB.getInvMass()) mB = mB * rrB + 1 / (m_rbB.getInvInertiaTensorWorld() * ax1).length();
 		}
-		btScalar m = mA > mB ? mB : mA;
-		btScalar angularfreq = sqrt(ks / m);
-
+		btScalar m;
+		if (m_rbA.getInvMass() == 0) m = mB; else
+		if (m_rbB.getInvMass() == 0) m = mA; else
+			m = mA*mB / (mA + mB);
+		btScalar angularfreq = btSqrt(ks / m);
 
 		//limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
-		if(limot->m_springStiffnessLimited && 0.25 < angularfreq * dt)
+		if (limot->m_springStiffnessLimited && 0.25 < angularfreq * dt)
 		{
 			ks = BT_ONE / dt / dt / btScalar(16.0) * m;
 		}
 		//avoid damping that would blow up the spring
-		if(limot->m_springDampingLimited && kd * dt > m)
+		if (limot->m_springDampingLimited && kd * dt > m)
 		{
 			kd = m / dt;
 		}
 		btScalar fs = ks * error * dt;
 		btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
-		btScalar f = (fs+fd);
-
-	// after the spring force affecting the body(es) the new velocity will be
-	// vel + f / m * (rotational ? -1 : 1)
-	// so in theory this should be set here for m_constraintError
-	// (with m_constraintError we set a desired velocity for the affected body(es))
-	// however in practice any value is fine as long as it is greater then the "proper" velocity,
-	// because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force
-	// so it is much simpler (and more robust) just to simply use inf (with the proper sign)
-	// you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration)
-	// will we not request a velocity with the wrong direction ?
-	// and the answare is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
-	// so the sign of the force that is really matters
-	info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
+		btScalar f = (fs + fd);
+
+		// after the spring force affecting the body(es) the new velocity will be
+		// vel + f / m * (rotational ? -1 : 1)
+		// so in theory this should be set here for m_constraintError
+		// (with m_constraintError we set a desired velocity for the affected body(es))
+		// however in practice any value is fine as long as it is greater than the "proper" velocity,
+		// because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force
+		// so it is much simpler (and more robust) just to simply use inf (with the proper sign)
+		// (Even with our best intent the "new" velocity is only an estimation. If we underestimate
+		// the "proper" velocity that will weaken the spring, however if we overestimate it, it doesn't
+		// matter, because the solver will limit it according the force limit)
+		// you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration)
+		// will we not request a velocity with the wrong direction ?
+		// and the answer is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
+		// so the sign of the force that is really matters
+		if (m_flags & BT_6DOF_FLAGS_USE_INFINITE_ERROR)
+			info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
+		else
+			info->m_constraintError[srow] = vel + f / m * (rotational ? -1 : 1);
 
 		btScalar minf = f < fd ? f : fd;
 		btScalar maxf = f < fd ? fd : f;
-		if(!rotational)
+		if (!rotational)
 		{
 			info->m_lowerLimit[srow] = minf > 0 ? 0 : minf;
 			info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf;
@@ -840,56 +902,55 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 	return count;
 }
 
-
-//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 //If no axis is provided, it uses the default axis for this constraint.
 void btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis)
 {
-	if((axis >= 0) && (axis < 3))
+	if ((axis >= 0) && (axis < 3))
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_STOP_ERP : 
+			case BT_CONSTRAINT_STOP_ERP:
 				m_linearLimits.m_stopERP[axis] = value;
 				m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
 				break;
-			case BT_CONSTRAINT_STOP_CFM : 
+			case BT_CONSTRAINT_STOP_CFM:
 				m_linearLimits.m_stopCFM[axis] = value;
 				m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
 				break;
-			case BT_CONSTRAINT_ERP : 
+			case BT_CONSTRAINT_ERP:
 				m_linearLimits.m_motorERP[axis] = value;
 				m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
 				break;
-			case BT_CONSTRAINT_CFM : 
+			case BT_CONSTRAINT_CFM:
 				m_linearLimits.m_motorCFM[axis] = value;
 				m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
 				break;
-			default : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
-	else if((axis >=3) && (axis < 6))
+	else if ((axis >= 3) && (axis < 6))
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_STOP_ERP : 
+			case BT_CONSTRAINT_STOP_ERP:
 				m_angularLimits[axis - 3].m_stopERP = value;
 				m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
 				break;
-			case BT_CONSTRAINT_STOP_CFM : 
+			case BT_CONSTRAINT_STOP_CFM:
 				m_angularLimits[axis - 3].m_stopCFM = value;
 				m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
 				break;
-			case BT_CONSTRAINT_ERP : 
+			case BT_CONSTRAINT_ERP:
 				m_angularLimits[axis - 3].m_motorERP = value;
 				m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
 				break;
-			case BT_CONSTRAINT_CFM : 
+			case BT_CONSTRAINT_CFM:
 				m_angularLimits[axis - 3].m_motorCFM = value;
 				m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
 				break;
-			default : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
@@ -900,54 +961,54 @@ void btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis)
 }
 
 //return the local value of parameter
-btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const 
+btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const
 {
 	btScalar retVal = 0;
-	if((axis >= 0) && (axis < 3))
+	if ((axis >= 0) && (axis < 3))
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_STOP_ERP : 
+			case BT_CONSTRAINT_STOP_ERP:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
 				retVal = m_linearLimits.m_stopERP[axis];
 				break;
-			case BT_CONSTRAINT_STOP_CFM : 
+			case BT_CONSTRAINT_STOP_CFM:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
 				retVal = m_linearLimits.m_stopCFM[axis];
 				break;
-			case BT_CONSTRAINT_ERP : 
+			case BT_CONSTRAINT_ERP:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
 				retVal = m_linearLimits.m_motorERP[axis];
 				break;
-			case BT_CONSTRAINT_CFM : 
+			case BT_CONSTRAINT_CFM:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
 				retVal = m_linearLimits.m_motorCFM[axis];
 				break;
-			default : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
-	else if((axis >=3) && (axis < 6))
+	else if ((axis >= 3) && (axis < 6))
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_STOP_ERP : 
+			case BT_CONSTRAINT_STOP_ERP:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
 				retVal = m_angularLimits[axis - 3].m_stopERP;
 				break;
-			case BT_CONSTRAINT_STOP_CFM : 
+			case BT_CONSTRAINT_STOP_CFM:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
 				retVal = m_angularLimits[axis - 3].m_stopCFM;
 				break;
-			case BT_CONSTRAINT_ERP : 
+			case BT_CONSTRAINT_ERP:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
 				retVal = m_angularLimits[axis - 3].m_motorERP;
 				break;
-			case BT_CONSTRAINT_CFM : 
+			case BT_CONSTRAINT_CFM:
 				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
 				retVal = m_angularLimits[axis - 3].m_motorCFM;
 				break;
-			default : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
@@ -958,31 +1019,29 @@ btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const
 	return retVal;
 }
 
- 
-
-void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1,const btVector3& axis2)
+void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1, const btVector3& axis2)
 {
 	btVector3 zAxis = axis1.normalized();
 	btVector3 yAxis = axis2.normalized();
-	btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
-	
+	btVector3 xAxis = yAxis.cross(zAxis);  // we want right coordinate system
+
 	btTransform frameInW;
 	frameInW.setIdentity();
-	frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
-	                              xAxis[1], yAxis[1], zAxis[1],
-	                              xAxis[2], yAxis[2], zAxis[2]);
-	
+	frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
+								 xAxis[1], yAxis[1], zAxis[1],
+								 xAxis[2], yAxis[2], zAxis[2]);
+
 	// now get constraint frame in local coordinate systems
 	m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
 	m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
-	
+
 	calculateTransforms();
 }
 
 void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3)
+	if (index < 3)
 		m_linearLimits.m_bounce[index] = bounce;
 	else
 		m_angularLimits[index - 3].m_bounce = bounce;
@@ -991,7 +1050,7 @@ void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce)
 void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3)
+	if (index < 3)
 		m_linearLimits.m_enableMotor[index] = onOff;
 	else
 		m_angularLimits[index - 3].m_enableMotor = onOff;
@@ -1000,7 +1059,7 @@ void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff)
 void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3)
+	if (index < 3)
 		m_linearLimits.m_servoMotor[index] = onOff;
 	else
 		m_angularLimits[index - 3].m_servoMotor = onOff;
@@ -1009,25 +1068,60 @@ void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
 void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar velocity)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3)
+	if (index < 3)
 		m_linearLimits.m_targetVelocity[index] = velocity;
 	else
 		m_angularLimits[index - 3].m_targetVelocity = velocity;
 }
 
-void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar target)
+void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOrg)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3)
-		m_linearLimits.m_servoTarget[index] = target;
+	if (index < 3)
+	{
+		m_linearLimits.m_servoTarget[index] = targetOrg;
+	}
 	else
+	{
+		//wrap between -PI and PI, see also
+		//https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi
+
+		btScalar target = targetOrg + SIMD_PI;
+		if (1)
+		{
+			btScalar m = target - SIMD_2_PI * std::floor(target / SIMD_2_PI);
+			// handle boundary cases resulted from floating-point cut off:
+			{
+				if (m >= SIMD_2_PI)
+				{
+					target = 0;
+				}
+				else
+				{
+					if (m < 0)
+					{
+						if (SIMD_2_PI + m == SIMD_2_PI)
+							target = 0;
+						else
+							target = SIMD_2_PI + m;
+					}
+					else
+					{
+						target = m;
+					}
+				}
+			}
+			target -= SIMD_PI;
+		}
+
 		m_angularLimits[index - 3].m_servoTarget = target;
+	}
 }
 
 void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3)
+	if (index < 3)
 		m_linearLimits.m_maxMotorForce[index] = force;
 	else
 		m_angularLimits[index - 3].m_maxMotorForce = force;
@@ -1036,19 +1130,22 @@ void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force)
 void btGeneric6DofSpring2Constraint::enableSpring(int index, bool onOff)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3)
+	if (index < 3)
 		m_linearLimits.m_enableSpring[index] = onOff;
 	else
-		m_angularLimits[index - 3] .m_enableSpring = onOff;
+		m_angularLimits[index - 3].m_enableSpring = onOff;
 }
 
 void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3) {
+	if (index < 3)
+	{
 		m_linearLimits.m_springStiffness[index] = stiffness;
 		m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded;
-	} else {
+	}
+	else
+	{
 		m_angularLimits[index - 3].m_springStiffness = stiffness;
 		m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded;
 	}
@@ -1057,10 +1154,13 @@ void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness,
 void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3) {
+	if (index < 3)
+	{
 		m_linearLimits.m_springDamping[index] = damping;
 		m_linearLimits.m_springDampingLimited[index] = limitIfNeeded;
-	} else {
+	}
+	else
+	{
 		m_angularLimits[index - 3].m_springDamping = damping;
 		m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded;
 	}
@@ -1070,9 +1170,9 @@ void btGeneric6DofSpring2Constraint::setEquilibriumPoint()
 {
 	calculateTransforms();
 	int i;
-	for( i = 0; i < 3; i++)
+	for (i = 0; i < 3; i++)
 		m_linearLimits.m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
-	for(i = 0; i < 3; i++)
+	for (i = 0; i < 3; i++)
 		m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i];
 }
 
@@ -1080,35 +1180,38 @@ void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index)
 {
 	btAssert((index >= 0) && (index < 6));
 	calculateTransforms();
-	if (index<3)
+	if (index < 3)
 		m_linearLimits.m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
 	else
-		m_angularLimits[index - 3] .m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3];
+		m_angularLimits[index - 3].m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3];
 }
 
 void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index, btScalar val)
 {
 	btAssert((index >= 0) && (index < 6));
-	if (index<3)
+	if (index < 3)
 		m_linearLimits.m_equilibriumPoint[index] = val;
 	else
-		m_angularLimits[index - 3] .m_equilibriumPoint = val;
+		m_angularLimits[index - 3].m_equilibriumPoint = val;
 }
 
-
 //////////////////////////// btRotationalLimitMotor2 ////////////////////////////////////
 
 void btRotationalLimitMotor2::testLimitValue(btScalar test_value)
 {
 	//we can't normalize the angles here because we would lost the sign that we use later, but it doesn't seem to be a problem
-	if(m_loLimit > m_hiLimit) {
+	if (m_loLimit > m_hiLimit)
+	{
 		m_currentLimit = 0;
 		m_currentLimitError = btScalar(0.f);
 	}
-	else if(m_loLimit == m_hiLimit) {
+	else if (m_loLimit == m_hiLimit)
+	{
 		m_currentLimitError = test_value - m_loLimit;
 		m_currentLimit = 3;
-	} else {
+	}
+	else
+	{
 		m_currentLimitError = test_value - m_loLimit;
 		m_currentLimitErrorHi = test_value - m_hiLimit;
 		m_currentLimit = 4;
@@ -1121,18 +1224,20 @@ void btTranslationalLimitMotor2::testLimitValue(int limitIndex, btScalar test_va
 {
 	btScalar loLimit = m_lowerLimit[limitIndex];
 	btScalar hiLimit = m_upperLimit[limitIndex];
-	if(loLimit > hiLimit) {
+	if (loLimit > hiLimit)
+	{
 		m_currentLimitError[limitIndex] = 0;
 		m_currentLimit[limitIndex] = 0;
 	}
-	else if(loLimit == hiLimit) {
+	else if (loLimit == hiLimit)
+	{
 		m_currentLimitError[limitIndex] = test_value - loLimit;
 		m_currentLimit[limitIndex] = 3;
-	} else {
+	}
+	else
+	{
 		m_currentLimitError[limitIndex] = test_value - loLimit;
 		m_currentLimitErrorHi[limitIndex] = test_value - hiLimit;
 		m_currentLimit[limitIndex] = 4;
 	}
 }
-
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
index ace4b3c29bf..c86dc373da6 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
@@ -37,7 +37,6 @@ email: projectileman@yahoo.com
 http://gimpact.sf.net
 */
 
-
 #ifndef BT_GENERIC_6DOF_CONSTRAINT2_H
 #define BT_GENERIC_6DOF_CONSTRAINT2_H
 
@@ -47,18 +46,17 @@ http://gimpact.sf.net
 
 class btRigidBody;
 
-
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btGeneric6DofSpring2ConstraintData2		btGeneric6DofSpring2ConstraintDoubleData2
-#define btGeneric6DofSpring2ConstraintDataName	"btGeneric6DofSpring2ConstraintDoubleData2"
+#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintDoubleData2
+#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintDoubleData2"
 #else
-#define btGeneric6DofSpring2ConstraintData2		btGeneric6DofSpring2ConstraintData
-#define btGeneric6DofSpring2ConstraintDataName	"btGeneric6DofSpring2ConstraintData"
-#endif //BT_USE_DOUBLE_PRECISION
+#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintData
+#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
 enum RotateOrder
 {
-	RO_XYZ=0,
+	RO_XYZ = 0,
 	RO_XZY,
 	RO_YXZ,
 	RO_YZX,
@@ -69,9 +67,9 @@ enum RotateOrder
 class btRotationalLimitMotor2
 {
 public:
-// upper < lower means free
-// upper == lower means locked
-// upper > lower means limited
+	// upper < lower means free
+	// upper == lower means locked
+	// upper > lower means limited
 	btScalar m_loLimit;
 	btScalar m_hiLimit;
 	btScalar m_bounce;
@@ -79,95 +77,92 @@ public:
 	btScalar m_stopCFM;
 	btScalar m_motorERP;
 	btScalar m_motorCFM;
-	bool     m_enableMotor;
+	bool m_enableMotor;
 	btScalar m_targetVelocity;
 	btScalar m_maxMotorForce;
-	bool     m_servoMotor;
+	bool m_servoMotor;
 	btScalar m_servoTarget;
-	bool     m_enableSpring;
+	bool m_enableSpring;
 	btScalar m_springStiffness;
-	bool     m_springStiffnessLimited;
+	bool m_springStiffnessLimited;
 	btScalar m_springDamping;
-	bool     m_springDampingLimited;
+	bool m_springDampingLimited;
 	btScalar m_equilibriumPoint;
 
 	btScalar m_currentLimitError;
 	btScalar m_currentLimitErrorHi;
 	btScalar m_currentPosition;
-	int      m_currentLimit;
+	int m_currentLimit;
 
 	btRotationalLimitMotor2()
 	{
-		m_loLimit                = 1.0f;
-		m_hiLimit                = -1.0f;
-		m_bounce                 = 0.0f;
-		m_stopERP                = 0.2f;
-		m_stopCFM                = 0.f;
-		m_motorERP               = 0.9f;
-		m_motorCFM               = 0.f;
-		m_enableMotor            = false;
-		m_targetVelocity         = 0;
-		m_maxMotorForce          = 0.1f;
-		m_servoMotor             = false;
-		m_servoTarget            = 0;
-		m_enableSpring           = false;
-		m_springStiffness        = 0;
+		m_loLimit = 1.0f;
+		m_hiLimit = -1.0f;
+		m_bounce = 0.0f;
+		m_stopERP = 0.2f;
+		m_stopCFM = 0.f;
+		m_motorERP = 0.9f;
+		m_motorCFM = 0.f;
+		m_enableMotor = false;
+		m_targetVelocity = 0;
+		m_maxMotorForce = 6.0f;
+		m_servoMotor = false;
+		m_servoTarget = 0;
+		m_enableSpring = false;
+		m_springStiffness = 0;
 		m_springStiffnessLimited = false;
-		m_springDamping          = 0;
-		m_springDampingLimited   = false;
-		m_equilibriumPoint       = 0;
+		m_springDamping = 0;
+		m_springDampingLimited = false;
+		m_equilibriumPoint = 0;
 
-		m_currentLimitError   = 0;
+		m_currentLimitError = 0;
 		m_currentLimitErrorHi = 0;
-		m_currentPosition     = 0;
-		m_currentLimit        = 0;
+		m_currentPosition = 0;
+		m_currentLimit = 0;
 	}
 
-	btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot)
+	btRotationalLimitMotor2(const btRotationalLimitMotor2& limot)
 	{
-		m_loLimit                = limot.m_loLimit;
-		m_hiLimit                = limot.m_hiLimit;
-		m_bounce                 = limot.m_bounce;
-		m_stopERP                = limot.m_stopERP;
-		m_stopCFM                = limot.m_stopCFM;
-		m_motorERP               = limot.m_motorERP;
-		m_motorCFM               = limot.m_motorCFM;
-		m_enableMotor            = limot.m_enableMotor;
-		m_targetVelocity         = limot.m_targetVelocity;
-		m_maxMotorForce          = limot.m_maxMotorForce;
-		m_servoMotor             = limot.m_servoMotor;
-		m_servoTarget            = limot.m_servoTarget;
-		m_enableSpring           = limot.m_enableSpring;
-		m_springStiffness        = limot.m_springStiffness;
+		m_loLimit = limot.m_loLimit;
+		m_hiLimit = limot.m_hiLimit;
+		m_bounce = limot.m_bounce;
+		m_stopERP = limot.m_stopERP;
+		m_stopCFM = limot.m_stopCFM;
+		m_motorERP = limot.m_motorERP;
+		m_motorCFM = limot.m_motorCFM;
+		m_enableMotor = limot.m_enableMotor;
+		m_targetVelocity = limot.m_targetVelocity;
+		m_maxMotorForce = limot.m_maxMotorForce;
+		m_servoMotor = limot.m_servoMotor;
+		m_servoTarget = limot.m_servoTarget;
+		m_enableSpring = limot.m_enableSpring;
+		m_springStiffness = limot.m_springStiffness;
 		m_springStiffnessLimited = limot.m_springStiffnessLimited;
-		m_springDamping          = limot.m_springDamping;
-		m_springDampingLimited   = limot.m_springDampingLimited;
-		m_equilibriumPoint       = limot.m_equilibriumPoint;
+		m_springDamping = limot.m_springDamping;
+		m_springDampingLimited = limot.m_springDampingLimited;
+		m_equilibriumPoint = limot.m_equilibriumPoint;
 
-		m_currentLimitError   = limot.m_currentLimitError;
+		m_currentLimitError = limot.m_currentLimitError;
 		m_currentLimitErrorHi = limot.m_currentLimitErrorHi;
-		m_currentPosition     = limot.m_currentPosition;
-		m_currentLimit        = limot.m_currentLimit;
+		m_currentPosition = limot.m_currentPosition;
+		m_currentLimit = limot.m_currentLimit;
 	}
 
-
 	bool isLimited()
 	{
-		if(m_loLimit > m_hiLimit) return false;
+		if (m_loLimit > m_hiLimit) return false;
 		return true;
 	}
 
 	void testLimitValue(btScalar test_value);
 };
 
-
-
 class btTranslationalLimitMotor2
 {
 public:
-// upper < lower means free
-// upper == lower means locked
-// upper > lower means limited
+	// upper < lower means free
+	// upper == lower means locked
+	// upper > lower means limited
 	btVector3 m_lowerLimit;
 	btVector3 m_upperLimit;
 	btVector3 m_bounce;
@@ -175,14 +170,14 @@ public:
 	btVector3 m_stopCFM;
 	btVector3 m_motorERP;
 	btVector3 m_motorCFM;
-	bool      m_enableMotor[3];
-	bool      m_servoMotor[3];
-	bool      m_enableSpring[3];
+	bool m_enableMotor[3];
+	bool m_servoMotor[3];
+	bool m_enableSpring[3];
 	btVector3 m_servoTarget;
 	btVector3 m_springStiffness;
-	bool      m_springStiffnessLimited[3];
+	bool m_springStiffnessLimited[3];
 	btVector3 m_springDamping;
-	bool      m_springDampingLimited[3];
+	bool m_springDampingLimited[3];
 	btVector3 m_equilibriumPoint;
 	btVector3 m_targetVelocity;
 	btVector3 m_maxMotorForce;
@@ -190,69 +185,69 @@ public:
 	btVector3 m_currentLimitError;
 	btVector3 m_currentLimitErrorHi;
 	btVector3 m_currentLinearDiff;
-	int       m_currentLimit[3];
+	int m_currentLimit[3];
 
 	btTranslationalLimitMotor2()
 	{
-		m_lowerLimit         .setValue(0.f , 0.f , 0.f );
-		m_upperLimit         .setValue(0.f , 0.f , 0.f );
-		m_bounce             .setValue(0.f , 0.f , 0.f );
-		m_stopERP            .setValue(0.2f, 0.2f, 0.2f);
-		m_stopCFM            .setValue(0.f , 0.f , 0.f );
-		m_motorERP           .setValue(0.9f, 0.9f, 0.9f);
-		m_motorCFM           .setValue(0.f , 0.f , 0.f );
-
-		m_currentLimitError  .setValue(0.f , 0.f , 0.f );
-		m_currentLimitErrorHi.setValue(0.f , 0.f , 0.f );
-		m_currentLinearDiff  .setValue(0.f , 0.f , 0.f );
-
-		for(int i=0; i < 3; i++) 
+		m_lowerLimit.setValue(0.f, 0.f, 0.f);
+		m_upperLimit.setValue(0.f, 0.f, 0.f);
+		m_bounce.setValue(0.f, 0.f, 0.f);
+		m_stopERP.setValue(0.2f, 0.2f, 0.2f);
+		m_stopCFM.setValue(0.f, 0.f, 0.f);
+		m_motorERP.setValue(0.9f, 0.9f, 0.9f);
+		m_motorCFM.setValue(0.f, 0.f, 0.f);
+
+		m_currentLimitError.setValue(0.f, 0.f, 0.f);
+		m_currentLimitErrorHi.setValue(0.f, 0.f, 0.f);
+		m_currentLinearDiff.setValue(0.f, 0.f, 0.f);
+
+		for (int i = 0; i < 3; i++)
 		{
-			m_enableMotor[i]            = false;
-			m_servoMotor[i]             = false;
-			m_enableSpring[i]           = false;
-			m_servoTarget[i]            = btScalar(0.f);
-			m_springStiffness[i]        = btScalar(0.f);
+			m_enableMotor[i] = false;
+			m_servoMotor[i] = false;
+			m_enableSpring[i] = false;
+			m_servoTarget[i] = btScalar(0.f);
+			m_springStiffness[i] = btScalar(0.f);
 			m_springStiffnessLimited[i] = false;
-			m_springDamping[i]          = btScalar(0.f);
-			m_springDampingLimited[i]   = false;
-			m_equilibriumPoint[i]       = btScalar(0.f);
-			m_targetVelocity[i]         = btScalar(0.f);
-			m_maxMotorForce[i]          = btScalar(0.f);
-			
-			m_currentLimit[i]     = 0;
+			m_springDamping[i] = btScalar(0.f);
+			m_springDampingLimited[i] = false;
+			m_equilibriumPoint[i] = btScalar(0.f);
+			m_targetVelocity[i] = btScalar(0.f);
+			m_maxMotorForce[i] = btScalar(0.f);
+
+			m_currentLimit[i] = 0;
 		}
 	}
 
-	btTranslationalLimitMotor2(const btTranslationalLimitMotor2 & other )
+	btTranslationalLimitMotor2(const btTranslationalLimitMotor2& other)
 	{
-		m_lowerLimit          = other.m_lowerLimit;
-		m_upperLimit          = other.m_upperLimit;
-		m_bounce              = other.m_bounce;
-		m_stopERP             = other.m_stopERP;
-		m_stopCFM             = other.m_stopCFM;
-		m_motorERP            = other.m_motorERP;
-		m_motorCFM            = other.m_motorCFM;
-		
-		m_currentLimitError   = other.m_currentLimitError;
+		m_lowerLimit = other.m_lowerLimit;
+		m_upperLimit = other.m_upperLimit;
+		m_bounce = other.m_bounce;
+		m_stopERP = other.m_stopERP;
+		m_stopCFM = other.m_stopCFM;
+		m_motorERP = other.m_motorERP;
+		m_motorCFM = other.m_motorCFM;
+
+		m_currentLimitError = other.m_currentLimitError;
 		m_currentLimitErrorHi = other.m_currentLimitErrorHi;
-		m_currentLinearDiff   = other.m_currentLinearDiff;
+		m_currentLinearDiff = other.m_currentLinearDiff;
 
-		for(int i=0; i < 3; i++) 
+		for (int i = 0; i < 3; i++)
 		{
-			m_enableMotor[i]            = other.m_enableMotor[i];
-			m_servoMotor[i]             = other.m_servoMotor[i];
-			m_enableSpring[i]           = other.m_enableSpring[i];
-			m_servoTarget[i]            = other.m_servoTarget[i];
-			m_springStiffness[i]        = other.m_springStiffness[i];
+			m_enableMotor[i] = other.m_enableMotor[i];
+			m_servoMotor[i] = other.m_servoMotor[i];
+			m_enableSpring[i] = other.m_enableSpring[i];
+			m_servoTarget[i] = other.m_servoTarget[i];
+			m_springStiffness[i] = other.m_springStiffness[i];
 			m_springStiffnessLimited[i] = other.m_springStiffnessLimited[i];
-			m_springDamping[i]          = other.m_springDamping[i];
-			m_springDampingLimited[i]   = other.m_springDampingLimited[i];
-			m_equilibriumPoint[i]       = other.m_equilibriumPoint[i];
-			m_targetVelocity[i]         = other.m_targetVelocity[i];
-			m_maxMotorForce[i]          = other.m_maxMotorForce[i];
+			m_springDamping[i] = other.m_springDamping[i];
+			m_springDampingLimited[i] = other.m_springDampingLimited[i];
+			m_equilibriumPoint[i] = other.m_equilibriumPoint[i];
+			m_targetVelocity[i] = other.m_targetVelocity[i];
+			m_maxMotorForce[i] = other.m_maxMotorForce[i];
 
-			m_currentLimit[i]     = other.m_currentLimit[i];
+			m_currentLimit[i] = other.m_currentLimit[i];
 		}
 	}
 
@@ -269,15 +264,15 @@ enum bt6DofFlags2
 	BT_6DOF_FLAGS_CFM_STOP2 = 1,
 	BT_6DOF_FLAGS_ERP_STOP2 = 2,
 	BT_6DOF_FLAGS_CFM_MOTO2 = 4,
-	BT_6DOF_FLAGS_ERP_MOTO2 = 8
+	BT_6DOF_FLAGS_ERP_MOTO2 = 8,
+	BT_6DOF_FLAGS_USE_INFINITE_ERROR = (1<<16)
 };
-#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis
-
+#define BT_6DOF_FLAGS_AXIS_SHIFT2 4  // bits per axis
 
-ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpring2Constraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class)
+btGeneric6DofSpring2Constraint : public btTypedConstraint
 {
 protected:
-
 	btTransform m_frameInA;
 	btTransform m_frameInB;
 
@@ -290,53 +285,43 @@ protected:
 	RotateOrder m_rotateOrder;
 
 protected:
-
-	btTransform  m_calculatedTransformA;
-	btTransform  m_calculatedTransformB;
-	btVector3    m_calculatedAxisAngleDiff;
-	btVector3    m_calculatedAxis[3];
-	btVector3    m_calculatedLinearDiff;
-	btScalar     m_factA;
-	btScalar     m_factB;
-	bool         m_hasStaticBody;
-	int          m_flags;
-
-	btGeneric6DofSpring2Constraint&	operator=(btGeneric6DofSpring2Constraint&)
+	btTransform m_calculatedTransformA;
+	btTransform m_calculatedTransformB;
+	btVector3 m_calculatedAxisAngleDiff;
+	btVector3 m_calculatedAxis[3];
+	btVector3 m_calculatedLinearDiff;
+	btScalar m_factA;
+	btScalar m_factB;
+	bool m_hasStaticBody;
+	int m_flags;
+
+	btGeneric6DofSpring2Constraint& operator=(const btGeneric6DofSpring2Constraint&)
 	{
 		btAssert(0);
 		return *this;
 	}
 
-	int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
-	int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+	int setAngularLimits(btConstraintInfo2 * info, int row_offset, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB);
+	int setLinearLimits(btConstraintInfo2 * info, int row, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB);
 
 	void calculateLinearInfo();
 	void calculateAngleInfo();
 	void testAngularLimitMotor(int axis_index);
 
-	void calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed);
-	int get_limit_motor_info2(btRotationalLimitMotor2* limot,
-		const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
-		btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
-
-	static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
-	static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
-	static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz);
-	static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz);
-	static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz);
-	static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz);
-	static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz);
+	void calculateJacobi(btRotationalLimitMotor2 * limot, const btTransform& transA, const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed);
+	int get_limit_motor_info2(btRotationalLimitMotor2 * limot,
+							  const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB,
+							  btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-    btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
-    btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
+	btGeneric6DofSpring2Constraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
+	btGeneric6DofSpring2Constraint(btRigidBody & rbB, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ);
 
 	virtual void buildJacobian() {}
-	virtual void getInfo1 (btConstraintInfo1* info);
-	virtual void getInfo2 (btConstraintInfo2* info);
+	virtual void getInfo1(btConstraintInfo1 * info);
+	virtual void getInfo2(btConstraintInfo2 * info);
 	virtual int calculateSerializeBufferSize() const;
 	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 
@@ -344,19 +329,19 @@ public:
 	btTranslationalLimitMotor2* getTranslationalLimitMotor() { return &m_linearLimits; }
 
 	// Calculates the global transform for the joint offset for body A an B, and also calculates the angle differences between the bodies.
-	void calculateTransforms(const btTransform& transA,const btTransform& transB);
+	void calculateTransforms(const btTransform& transA, const btTransform& transB);
 	void calculateTransforms();
 
 	// Gets the global transform of the offset for body A
-	const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
+	const btTransform& getCalculatedTransformA() const { return m_calculatedTransformA; }
 	// Gets the global transform of the offset for body B
-	const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }
+	const btTransform& getCalculatedTransformB() const { return m_calculatedTransformB; }
 
-	const btTransform & getFrameOffsetA() const { return m_frameInA; }
-	const btTransform & getFrameOffsetB() const { return m_frameInB; }
+	const btTransform& getFrameOffsetA() const { return m_frameInA; }
+	const btTransform& getFrameOffsetB() const { return m_frameInB; }
 
-	btTransform & getFrameOffsetA() { return m_frameInA; }
-	btTransform & getFrameOffsetB() { return m_frameInB; }
+	btTransform& getFrameOffsetA() { return m_frameInA; }
+	btTransform& getFrameOffsetB() { return m_frameInB; }
 
 	// Get the rotation axis in global coordinates ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
 	btVector3 getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; }
@@ -367,58 +352,58 @@ public:
 	// Get the relative position of the constraint pivot ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously )
 	btScalar getRelativePivotPosition(int axis_index) const { return m_calculatedLinearDiff[axis_index]; }
 
-	void setFrames(const btTransform & frameA, const btTransform & frameB);
+	void setFrames(const btTransform& frameA, const btTransform& frameB);
 
 	void setLinearLowerLimit(const btVector3& linearLower) { m_linearLimits.m_lowerLimit = linearLower; }
-	void getLinearLowerLimit(btVector3& linearLower) { linearLower = m_linearLimits.m_lowerLimit; }
+	void getLinearLowerLimit(btVector3 & linearLower) { linearLower = m_linearLimits.m_lowerLimit; }
 	void setLinearUpperLimit(const btVector3& linearUpper) { m_linearLimits.m_upperLimit = linearUpper; }
-	void getLinearUpperLimit(btVector3& linearUpper) { linearUpper = m_linearLimits.m_upperLimit; }
+	void getLinearUpperLimit(btVector3 & linearUpper) { linearUpper = m_linearLimits.m_upperLimit; }
 
 	void setAngularLowerLimit(const btVector3& angularLower)
 	{
-		for(int i = 0; i < 3; i++) 
+		for (int i = 0; i < 3; i++)
 			m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
 	}
 
 	void setAngularLowerLimitReversed(const btVector3& angularLower)
 	{
-		for(int i = 0; i < 3; i++) 
+		for (int i = 0; i < 3; i++)
 			m_angularLimits[i].m_hiLimit = btNormalizeAngle(-angularLower[i]);
 	}
 
-	void getAngularLowerLimit(btVector3& angularLower)
+	void getAngularLowerLimit(btVector3 & angularLower)
 	{
-		for(int i = 0; i < 3; i++) 
+		for (int i = 0; i < 3; i++)
 			angularLower[i] = m_angularLimits[i].m_loLimit;
 	}
 
-	void getAngularLowerLimitReversed(btVector3& angularLower)
+	void getAngularLowerLimitReversed(btVector3 & angularLower)
 	{
-		for(int i = 0; i < 3; i++)
+		for (int i = 0; i < 3; i++)
 			angularLower[i] = -m_angularLimits[i].m_hiLimit;
 	}
 
 	void setAngularUpperLimit(const btVector3& angularUpper)
 	{
-		for(int i = 0; i < 3; i++)
+		for (int i = 0; i < 3; i++)
 			m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
 	}
 
 	void setAngularUpperLimitReversed(const btVector3& angularUpper)
 	{
-		for(int i = 0; i < 3; i++)
+		for (int i = 0; i < 3; i++)
 			m_angularLimits[i].m_loLimit = btNormalizeAngle(-angularUpper[i]);
 	}
 
-	void getAngularUpperLimit(btVector3& angularUpper)
+	void getAngularUpperLimit(btVector3 & angularUpper)
 	{
-		for(int i = 0; i < 3; i++)
+		for (int i = 0; i < 3; i++)
 			angularUpper[i] = m_angularLimits[i].m_hiLimit;
 	}
 
-	void getAngularUpperLimitReversed(btVector3& angularUpper)
+	void getAngularUpperLimitReversed(btVector3 & angularUpper)
 	{
-		for(int i = 0; i < 3; i++)
+		for (int i = 0; i < 3; i++)
 			angularUpper[i] = -m_angularLimits[i].m_loLimit;
 	}
 
@@ -426,7 +411,7 @@ public:
 
 	void setLimit(int axis, btScalar lo, btScalar hi)
 	{
-		if(axis<3)
+		if (axis < 3)
 		{
 			m_linearLimits.m_lowerLimit[axis] = lo;
 			m_linearLimits.m_upperLimit[axis] = hi;
@@ -435,14 +420,14 @@ public:
 		{
 			lo = btNormalizeAngle(lo);
 			hi = btNormalizeAngle(hi);
-			m_angularLimits[axis-3].m_loLimit = lo;
-			m_angularLimits[axis-3].m_hiLimit = hi;
+			m_angularLimits[axis - 3].m_loLimit = lo;
+			m_angularLimits[axis - 3].m_hiLimit = hi;
 		}
 	}
 
 	void setLimitReversed(int axis, btScalar lo, btScalar hi)
 	{
-		if(axis<3)
+		if (axis < 3)
 		{
 			m_linearLimits.m_lowerLimit[axis] = lo;
 			m_linearLimits.m_upperLimit[axis] = hi;
@@ -451,46 +436,53 @@ public:
 		{
 			lo = btNormalizeAngle(lo);
 			hi = btNormalizeAngle(hi);
-			m_angularLimits[axis-3].m_hiLimit = -lo;
-			m_angularLimits[axis-3].m_loLimit = -hi;
+			m_angularLimits[axis - 3].m_hiLimit = -lo;
+			m_angularLimits[axis - 3].m_loLimit = -hi;
 		}
 	}
 
 	bool isLimited(int limitIndex)
 	{
-		if(limitIndex<3)
+		if (limitIndex < 3)
 		{
 			return m_linearLimits.isLimited(limitIndex);
 		}
-		return m_angularLimits[limitIndex-3].isLimited();
+		return m_angularLimits[limitIndex - 3].isLimited();
 	}
 
 	void setRotationOrder(RotateOrder order) { m_rotateOrder = order; }
 	RotateOrder getRotationOrder() { return m_rotateOrder; }
 
-	void setAxis( const btVector3& axis1, const btVector3& axis2);
+	void setAxis(const btVector3& axis1, const btVector3& axis2);
 
 	void setBounce(int index, btScalar bounce);
 
 	void enableMotor(int index, bool onOff);
-	void setServo(int index, bool onOff); // set the type of the motor (servo or not) (the motor has to be turned on for servo also)
+	void setServo(int index, bool onOff);  // set the type of the motor (servo or not) (the motor has to be turned on for servo also)
 	void setTargetVelocity(int index, btScalar velocity);
 	void setServoTarget(int index, btScalar target);
 	void setMaxMotorForce(int index, btScalar force);
 
 	void enableSpring(int index, bool onOff);
-	void setStiffness(int index, btScalar stiffness, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the stiffness in necessary situations where otherwise the spring would move unrealistically too widely
-	void setDamping(int index, btScalar damping, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the damping in necessary situations where otherwise the spring would blow up
-	void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
-	void setEquilibriumPoint(int index);  // set the current constraint position/orientation as an equilibrium point for given DOF
+	void setStiffness(int index, btScalar stiffness, bool limitIfNeeded = true);  // if limitIfNeeded is true the system will automatically limit the stiffness in necessary situations where otherwise the spring would move unrealistically too widely
+	void setDamping(int index, btScalar damping, bool limitIfNeeded = true);      // if limitIfNeeded is true the system will automatically limit the damping in necessary situations where otherwise the spring would blow up
+	void setEquilibriumPoint();                                                   // set the current constraint position/orientation as an equilibrium point for all DOF
+	void setEquilibriumPoint(int index);                                          // set the current constraint position/orientation as an equilibrium point for given DOF
 	void setEquilibriumPoint(int index, btScalar val);
 
-	//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 	//If no axis is provided, it uses the default axis for this constraint.
 	virtual void setParam(int num, btScalar value, int axis = -1);
 	virtual btScalar getParam(int num, int axis = -1) const;
-};
 
+	static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
+	static bool matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz);
+	static bool matrixToEulerXZY(const btMatrix3x3& mat, btVector3& xyz);
+	static bool matrixToEulerYXZ(const btMatrix3x3& mat, btVector3& xyz);
+	static bool matrixToEulerYZX(const btMatrix3x3& mat, btVector3& xyz);
+	static bool matrixToEulerZXY(const btMatrix3x3& mat, btVector3& xyz);
+	static bool matrixToEulerZYX(const btMatrix3x3& mat, btVector3& xyz);
+};
 
 struct btGeneric6DofSpring2ConstraintData
 {
@@ -511,12 +503,12 @@ struct btGeneric6DofSpring2ConstraintData
 	btVector3FloatData m_linearSpringStiffness;
 	btVector3FloatData m_linearSpringDamping;
 	btVector3FloatData m_linearEquilibriumPoint;
-	char               m_linearEnableMotor[4];
-	char               m_linearServoMotor[4];
-	char               m_linearEnableSpring[4];
-	char               m_linearSpringStiffnessLimited[4];
-	char               m_linearSpringDampingLimited[4];
-	char               m_padding1[4];
+	char m_linearEnableMotor[4];
+	char m_linearServoMotor[4];
+	char m_linearEnableSpring[4];
+	char m_linearSpringStiffnessLimited[4];
+	char m_linearSpringDampingLimited[4];
+	char m_padding1[4];
 
 	btVector3FloatData m_angularUpperLimit;
 	btVector3FloatData m_angularLowerLimit;
@@ -531,13 +523,13 @@ struct btGeneric6DofSpring2ConstraintData
 	btVector3FloatData m_angularSpringStiffness;
 	btVector3FloatData m_angularSpringDamping;
 	btVector3FloatData m_angularEquilibriumPoint;
-	char               m_angularEnableMotor[4];
-	char               m_angularServoMotor[4];
-	char               m_angularEnableSpring[4];
-	char               m_angularSpringStiffnessLimited[4];
-	char               m_angularSpringDampingLimited[4];
+	char m_angularEnableMotor[4];
+	char m_angularServoMotor[4];
+	char m_angularEnableSpring[4];
+	char m_angularSpringStiffnessLimited[4];
+	char m_angularSpringDampingLimited[4];
 
-	int                m_rotateOrder;
+	int m_rotateOrder;
 };
 
 struct btGeneric6DofSpring2ConstraintDoubleData2
@@ -559,12 +551,12 @@ struct btGeneric6DofSpring2ConstraintDoubleData2
 	btVector3DoubleData m_linearSpringStiffness;
 	btVector3DoubleData m_linearSpringDamping;
 	btVector3DoubleData m_linearEquilibriumPoint;
-	char                m_linearEnableMotor[4];
-	char                m_linearServoMotor[4];
-	char                m_linearEnableSpring[4];
-	char                m_linearSpringStiffnessLimited[4];
-	char                m_linearSpringDampingLimited[4];
-	char                m_padding1[4];
+	char m_linearEnableMotor[4];
+	char m_linearServoMotor[4];
+	char m_linearEnableSpring[4];
+	char m_linearSpringStiffnessLimited[4];
+	char m_linearSpringDampingLimited[4];
+	char m_padding1[4];
 
 	btVector3DoubleData m_angularUpperLimit;
 	btVector3DoubleData m_angularLowerLimit;
@@ -579,13 +571,13 @@ struct btGeneric6DofSpring2ConstraintDoubleData2
 	btVector3DoubleData m_angularSpringStiffness;
 	btVector3DoubleData m_angularSpringDamping;
 	btVector3DoubleData m_angularEquilibriumPoint;
-	char                m_angularEnableMotor[4];
-	char                m_angularServoMotor[4];
-	char                m_angularEnableSpring[4];
-	char                m_angularSpringStiffnessLimited[4];
-	char                m_angularSpringDampingLimited[4];
+	char m_angularEnableMotor[4];
+	char m_angularServoMotor[4];
+	char m_angularEnableSpring[4];
+	char m_angularSpringStiffnessLimited[4];
+	char m_angularSpringDampingLimited[4];
 
-	int                 m_rotateOrder;
+	int m_rotateOrder;
 };
 
 SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const
@@ -596,79 +588,80 @@ SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSi
 SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 	btGeneric6DofSpring2ConstraintData2* dof = (btGeneric6DofSpring2ConstraintData2*)dataBuffer;
-	btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
+	btTypedConstraint::serialize(&dof->m_typeConstraintData, serializer);
 
 	m_frameInA.serialize(dof->m_rbAFrame);
 	m_frameInB.serialize(dof->m_rbBFrame);
 
 	int i;
-	for (i=0;i<3;i++)
+	for (i = 0; i < 3; i++)
 	{
-		dof->m_angularLowerLimit.m_floats[i]       = m_angularLimits[i].m_loLimit;
-		dof->m_angularUpperLimit.m_floats[i]       = m_angularLimits[i].m_hiLimit;
-		dof->m_angularBounce.m_floats[i]           = m_angularLimits[i].m_bounce;
-		dof->m_angularStopERP.m_floats[i]          = m_angularLimits[i].m_stopERP;
-		dof->m_angularStopCFM.m_floats[i]          = m_angularLimits[i].m_stopCFM;
-		dof->m_angularMotorERP.m_floats[i]         = m_angularLimits[i].m_motorERP;
-		dof->m_angularMotorCFM.m_floats[i]         = m_angularLimits[i].m_motorCFM;
-		dof->m_angularTargetVelocity.m_floats[i]   = m_angularLimits[i].m_targetVelocity;
-		dof->m_angularMaxMotorForce.m_floats[i]    = m_angularLimits[i].m_maxMotorForce;
-		dof->m_angularServoTarget.m_floats[i]      = m_angularLimits[i].m_servoTarget;
-		dof->m_angularSpringStiffness.m_floats[i]  = m_angularLimits[i].m_springStiffness;
-		dof->m_angularSpringDamping.m_floats[i]    = m_angularLimits[i].m_springDamping;
+		dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit;
+		dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit;
+		dof->m_angularBounce.m_floats[i] = m_angularLimits[i].m_bounce;
+		dof->m_angularStopERP.m_floats[i] = m_angularLimits[i].m_stopERP;
+		dof->m_angularStopCFM.m_floats[i] = m_angularLimits[i].m_stopCFM;
+		dof->m_angularMotorERP.m_floats[i] = m_angularLimits[i].m_motorERP;
+		dof->m_angularMotorCFM.m_floats[i] = m_angularLimits[i].m_motorCFM;
+		dof->m_angularTargetVelocity.m_floats[i] = m_angularLimits[i].m_targetVelocity;
+		dof->m_angularMaxMotorForce.m_floats[i] = m_angularLimits[i].m_maxMotorForce;
+		dof->m_angularServoTarget.m_floats[i] = m_angularLimits[i].m_servoTarget;
+		dof->m_angularSpringStiffness.m_floats[i] = m_angularLimits[i].m_springStiffness;
+		dof->m_angularSpringDamping.m_floats[i] = m_angularLimits[i].m_springDamping;
 		dof->m_angularEquilibriumPoint.m_floats[i] = m_angularLimits[i].m_equilibriumPoint;
 	}
-	dof->m_angularLowerLimit.m_floats[3]       = 0;
-	dof->m_angularUpperLimit.m_floats[3]       = 0;
-	dof->m_angularBounce.m_floats[3]           = 0;
-	dof->m_angularStopERP.m_floats[3]          = 0;
-	dof->m_angularStopCFM.m_floats[3]          = 0;
-	dof->m_angularMotorERP.m_floats[3]         = 0;
-	dof->m_angularMotorCFM.m_floats[3]         = 0;
-	dof->m_angularTargetVelocity.m_floats[3]   = 0;
-	dof->m_angularMaxMotorForce.m_floats[3]    = 0;
-	dof->m_angularServoTarget.m_floats[3]      = 0;
-	dof->m_angularSpringStiffness.m_floats[3]  = 0;
-	dof->m_angularSpringDamping.m_floats[3]    = 0;
+	dof->m_angularLowerLimit.m_floats[3] = 0;
+	dof->m_angularUpperLimit.m_floats[3] = 0;
+	dof->m_angularBounce.m_floats[3] = 0;
+	dof->m_angularStopERP.m_floats[3] = 0;
+	dof->m_angularStopCFM.m_floats[3] = 0;
+	dof->m_angularMotorERP.m_floats[3] = 0;
+	dof->m_angularMotorCFM.m_floats[3] = 0;
+	dof->m_angularTargetVelocity.m_floats[3] = 0;
+	dof->m_angularMaxMotorForce.m_floats[3] = 0;
+	dof->m_angularServoTarget.m_floats[3] = 0;
+	dof->m_angularSpringStiffness.m_floats[3] = 0;
+	dof->m_angularSpringDamping.m_floats[3] = 0;
 	dof->m_angularEquilibriumPoint.m_floats[3] = 0;
-	for (i=0;i<4;i++)
+	for (i = 0; i < 4; i++)
 	{
-		dof->m_angularEnableMotor[i]            = i < 3 ? ( m_angularLimits[i].m_enableMotor ? 1 : 0 ) : 0;
-		dof->m_angularServoMotor[i]             = i < 3 ? ( m_angularLimits[i].m_servoMotor ? 1 : 0 ) : 0;
-		dof->m_angularEnableSpring[i]           = i < 3 ? ( m_angularLimits[i].m_enableSpring ? 1 : 0 ) : 0;
-		dof->m_angularSpringStiffnessLimited[i] = i < 3 ? ( m_angularLimits[i].m_springStiffnessLimited ? 1 : 0 ) : 0;
-		dof->m_angularSpringDampingLimited[i]   = i < 3 ? ( m_angularLimits[i].m_springDampingLimited ? 1 : 0 ) : 0;
+		dof->m_angularEnableMotor[i] = i < 3 ? (m_angularLimits[i].m_enableMotor ? 1 : 0) : 0;
+		dof->m_angularServoMotor[i] = i < 3 ? (m_angularLimits[i].m_servoMotor ? 1 : 0) : 0;
+		dof->m_angularEnableSpring[i] = i < 3 ? (m_angularLimits[i].m_enableSpring ? 1 : 0) : 0;
+		dof->m_angularSpringStiffnessLimited[i] = i < 3 ? (m_angularLimits[i].m_springStiffnessLimited ? 1 : 0) : 0;
+		dof->m_angularSpringDampingLimited[i] = i < 3 ? (m_angularLimits[i].m_springDampingLimited ? 1 : 0) : 0;
 	}
 
-	m_linearLimits.m_lowerLimit.serialize( dof->m_linearLowerLimit );
-	m_linearLimits.m_upperLimit.serialize( dof->m_linearUpperLimit );
-	m_linearLimits.m_bounce.serialize( dof->m_linearBounce );
-	m_linearLimits.m_stopERP.serialize( dof->m_linearStopERP );
-	m_linearLimits.m_stopCFM.serialize( dof->m_linearStopCFM );
-	m_linearLimits.m_motorERP.serialize( dof->m_linearMotorERP );
-	m_linearLimits.m_motorCFM.serialize( dof->m_linearMotorCFM );
-	m_linearLimits.m_targetVelocity.serialize( dof->m_linearTargetVelocity );
-	m_linearLimits.m_maxMotorForce.serialize( dof->m_linearMaxMotorForce );
-	m_linearLimits.m_servoTarget.serialize( dof->m_linearServoTarget );
-	m_linearLimits.m_springStiffness.serialize( dof->m_linearSpringStiffness );
-	m_linearLimits.m_springDamping.serialize( dof->m_linearSpringDamping );
-	m_linearLimits.m_equilibriumPoint.serialize( dof->m_linearEquilibriumPoint );
-	for (i=0;i<4;i++)
+	m_linearLimits.m_lowerLimit.serialize(dof->m_linearLowerLimit);
+	m_linearLimits.m_upperLimit.serialize(dof->m_linearUpperLimit);
+	m_linearLimits.m_bounce.serialize(dof->m_linearBounce);
+	m_linearLimits.m_stopERP.serialize(dof->m_linearStopERP);
+	m_linearLimits.m_stopCFM.serialize(dof->m_linearStopCFM);
+	m_linearLimits.m_motorERP.serialize(dof->m_linearMotorERP);
+	m_linearLimits.m_motorCFM.serialize(dof->m_linearMotorCFM);
+	m_linearLimits.m_targetVelocity.serialize(dof->m_linearTargetVelocity);
+	m_linearLimits.m_maxMotorForce.serialize(dof->m_linearMaxMotorForce);
+	m_linearLimits.m_servoTarget.serialize(dof->m_linearServoTarget);
+	m_linearLimits.m_springStiffness.serialize(dof->m_linearSpringStiffness);
+	m_linearLimits.m_springDamping.serialize(dof->m_linearSpringDamping);
+	m_linearLimits.m_equilibriumPoint.serialize(dof->m_linearEquilibriumPoint);
+	for (i = 0; i < 4; i++)
 	{
-		dof->m_linearEnableMotor[i]            = i < 3 ? ( m_linearLimits.m_enableMotor[i] ? 1 : 0 ) : 0;
-		dof->m_linearServoMotor[i]             = i < 3 ? ( m_linearLimits.m_servoMotor[i] ? 1 : 0 ) : 0;
-		dof->m_linearEnableSpring[i]           = i < 3 ? ( m_linearLimits.m_enableSpring[i] ? 1 : 0 ) : 0;
-		dof->m_linearSpringStiffnessLimited[i] = i < 3 ? ( m_linearLimits.m_springStiffnessLimited[i] ? 1 : 0 ) : 0;
-		dof->m_linearSpringDampingLimited[i]   = i < 3 ? ( m_linearLimits.m_springDampingLimited[i] ? 1 : 0 ) : 0;
+		dof->m_linearEnableMotor[i] = i < 3 ? (m_linearLimits.m_enableMotor[i] ? 1 : 0) : 0;
+		dof->m_linearServoMotor[i] = i < 3 ? (m_linearLimits.m_servoMotor[i] ? 1 : 0) : 0;
+		dof->m_linearEnableSpring[i] = i < 3 ? (m_linearLimits.m_enableSpring[i] ? 1 : 0) : 0;
+		dof->m_linearSpringStiffnessLimited[i] = i < 3 ? (m_linearLimits.m_springStiffnessLimited[i] ? 1 : 0) : 0;
+		dof->m_linearSpringDampingLimited[i] = i < 3 ? (m_linearLimits.m_springDampingLimited[i] ? 1 : 0) : 0;
 	}
 
 	dof->m_rotateOrder = m_rotateOrder;
 
+	dof->m_padding1[0] = 0;
+	dof->m_padding1[1] = 0;
+	dof->m_padding1[2] = 0;
+	dof->m_padding1[3] = 0;
+
 	return btGeneric6DofSpring2ConstraintDataName;
 }
 
-
-
-
-
-#endif //BT_GENERIC_6DOF_CONSTRAINT_H
+#endif  //BT_GENERIC_6DOF_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp
index 6f765884ec0..8baf52bcd19 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp
@@ -17,26 +17,23 @@ subject to the following restrictions:
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
 
-
-btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA)
+btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
 	: btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
 {
-    init();
+	init();
 }
 
-
 btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
-        : btGeneric6DofConstraint(rbB, frameInB, useLinearReferenceFrameB)
+	: btGeneric6DofConstraint(rbB, frameInB, useLinearReferenceFrameB)
 {
-    init();
+	init();
 }
 
-
 void btGeneric6DofSpringConstraint::init()
 {
 	m_objectType = D6_SPRING_CONSTRAINT_TYPE;
 
-	for(int i = 0; i < 6; i++)
+	for (int i = 0; i < 6; i++)
 	{
 		m_springEnabled[i] = false;
 		m_equilibriumPoint[i] = btScalar(0.f);
@@ -45,12 +42,11 @@ void btGeneric6DofSpringConstraint::init()
 	}
 }
 
-
 void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
 {
 	btAssert((index >= 0) && (index < 6));
 	m_springEnabled[index] = onOff;
-	if(index < 3)
+	if (index < 3)
 	{
 		m_linearLimits.m_enableMotor[index] = onOff;
 	}
@@ -60,44 +56,38 @@ void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
 	}
 }
 
-
-
 void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness)
 {
 	btAssert((index >= 0) && (index < 6));
 	m_springStiffness[index] = stiffness;
 }
 
-
 void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping)
 {
 	btAssert((index >= 0) && (index < 6));
 	m_springDamping[index] = damping;
 }
 
-
 void btGeneric6DofSpringConstraint::setEquilibriumPoint()
 {
 	calculateTransforms();
 	int i;
 
-	for( i = 0; i < 3; i++)
+	for (i = 0; i < 3; i++)
 	{
 		m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
 	}
-	for(i = 0; i < 3; i++)
+	for (i = 0; i < 3; i++)
 	{
 		m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i];
 	}
 }
 
-
-
 void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
 {
 	btAssert((index >= 0) && (index < 6));
 	calculateTransforms();
-	if(index < 3)
+	if (index < 3)
 	{
 		m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
 	}
@@ -113,15 +103,14 @@ void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index, btScalar val)
 	m_equilibriumPoint[index] = val;
 }
 
-
 void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
 {
 	// it is assumed that calculateTransforms() have been called before this call
 	int i;
 	//btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
-	for(i = 0; i < 3; i++)
+	for (i = 0; i < 3; i++)
 	{
-		if(m_springEnabled[i])
+		if (m_springEnabled[i])
 		{
 			// get current position of constraint
 			btScalar currPos = m_calculatedLinearDiff[i];
@@ -130,28 +119,27 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf
 			// spring force is (delta * m_stiffness) according to Hooke's Law
 			btScalar force = delta * m_springStiffness[i];
 			btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations);
-			m_linearLimits.m_targetVelocity[i] =  velFactor * force;
-			m_linearLimits.m_maxMotorForce[i] =  btFabs(force) / info->fps;
+			m_linearLimits.m_targetVelocity[i] = velFactor * force;
+			m_linearLimits.m_maxMotorForce[i] = btFabs(force);
 		}
 	}
-	for(i = 0; i < 3; i++)
+	for (i = 0; i < 3; i++)
 	{
-		if(m_springEnabled[i + 3])
+		if (m_springEnabled[i + 3])
 		{
 			// get current position of constraint
 			btScalar currPos = m_calculatedAxisAngleDiff[i];
 			// calculate difference
-			btScalar delta = currPos - m_equilibriumPoint[i+3];
+			btScalar delta = currPos - m_equilibriumPoint[i + 3];
 			// spring force is (-delta * m_stiffness) according to Hooke's Law
-			btScalar force = -delta * m_springStiffness[i+3];
-			btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations);
+			btScalar force = -delta * m_springStiffness[i + 3];
+			btScalar velFactor = info->fps * m_springDamping[i + 3] / btScalar(info->m_numIterations);
 			m_angularLimits[i].m_targetVelocity = velFactor * force;
-			m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
+			m_angularLimits[i].m_maxMotorForce = btFabs(force);
 		}
 	}
 }
 
-
 void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
 {
 	// this will be called by constraint solver at the constraint setup stage
@@ -161,25 +149,21 @@ void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
 	btGeneric6DofConstraint::getInfo2(info);
 }
 
-
-void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
+void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1, const btVector3& axis2)
 {
 	btVector3 zAxis = axis1.normalized();
 	btVector3 yAxis = axis2.normalized();
-	btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
+	btVector3 xAxis = yAxis.cross(zAxis);  // we want right coordinate system
 
 	btTransform frameInW;
 	frameInW.setIdentity();
-	frameInW.getBasis().setValue(	xAxis[0], yAxis[0], zAxis[0],	
-                                xAxis[1], yAxis[1], zAxis[1],
-                                xAxis[2], yAxis[2], zAxis[2]);
+	frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
+								 xAxis[1], yAxis[1], zAxis[1],
+								 xAxis[2], yAxis[2], zAxis[2]);
 
 	// now get constraint frame in local coordinate systems
 	m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
 	m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
 
-  calculateTransforms();
+	calculateTransforms();
 }
-
-
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
index dac59c6889d..02b9d4d05d1 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
@@ -16,20 +16,17 @@ subject to the following restrictions:
 #ifndef BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
 #define BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
 
-
 #include "LinearMath/btVector3.h"
 #include "btTypedConstraint.h"
 #include "btGeneric6DofConstraint.h"
 
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btGeneric6DofSpringConstraintData2		btGeneric6DofSpringConstraintDoubleData2
-#define btGeneric6DofSpringConstraintDataName	"btGeneric6DofSpringConstraintDoubleData2"
+#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintDoubleData2
+#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintDoubleData2"
 #else
-#define btGeneric6DofSpringConstraintData2		btGeneric6DofSpringConstraintData
-#define btGeneric6DofSpringConstraintDataName	"btGeneric6DofSpringConstraintData"
-#endif //BT_USE_DOUBLE_PRECISION
-
-
+#define btGeneric6DofSpringConstraintData2 btGeneric6DofSpringConstraintData
+#define btGeneric6DofSpringConstraintDataName "btGeneric6DofSpringConstraintData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
 /// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF
 
@@ -41,101 +38,98 @@ subject to the following restrictions:
 /// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] )
 /// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] )
 
-ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
+ATTRIBUTE_ALIGNED16(class)
+btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
 {
 protected:
-	bool		m_springEnabled[6];
-	btScalar	m_equilibriumPoint[6];
-	btScalar	m_springStiffness[6];
-	btScalar	m_springDamping[6]; // between 0 and 1 (1 == no damping)
+	bool m_springEnabled[6];
+	btScalar m_equilibriumPoint[6];
+	btScalar m_springStiffness[6];
+	btScalar m_springDamping[6];  // between 0 and 1 (1 == no damping)
 	void init();
-	void internalUpdateSprings(btConstraintInfo2* info);
-public: 
-	
+	void internalUpdateSprings(btConstraintInfo2 * info);
+
+public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
-    btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
-    btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
+
+	btGeneric6DofSpringConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA);
+	btGeneric6DofSpringConstraint(btRigidBody & rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
 	void enableSpring(int index, bool onOff);
 	void setStiffness(int index, btScalar stiffness);
 	void setDamping(int index, btScalar damping);
-	void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
+	void setEquilibriumPoint();           // set the current constraint position/orientation as an equilibrium point for all DOF
 	void setEquilibriumPoint(int index);  // set the current constraint position/orientation as an equilibrium point for given DOF
 	void setEquilibriumPoint(int index, btScalar val);
 
 	bool isSpringEnabled(int index) const
 	{
-	    return m_springEnabled[index];
+		return m_springEnabled[index];
 	}
 
 	btScalar getStiffness(int index) const
 	{
-	    return m_springStiffness[index];
+		return m_springStiffness[index];
 	}
 
 	btScalar getDamping(int index) const
 	{
-	    return m_springDamping[index];
+		return m_springDamping[index];
 	}
 
 	btScalar getEquilibriumPoint(int index) const
 	{
-	    return m_equilibriumPoint[index];
+		return m_equilibriumPoint[index];
 	}
 
-	virtual void setAxis( const btVector3& axis1, const btVector3& axis2);
+	virtual void setAxis(const btVector3& axis1, const btVector3& axis2);
 
-	virtual void getInfo2 (btConstraintInfo2* info);
+	virtual void getInfo2(btConstraintInfo2 * info);
 
-	virtual	int	calculateSerializeBufferSize() const;
+	virtual int calculateSerializeBufferSize() const;
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
-
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
-
 struct btGeneric6DofSpringConstraintData
 {
-	btGeneric6DofConstraintData	m_6dofData;
-	
-	int			m_springEnabled[6];
-	float		m_equilibriumPoint[6];
-	float		m_springStiffness[6];
-	float		m_springDamping[6];
+	btGeneric6DofConstraintData m_6dofData;
+
+	int m_springEnabled[6];
+	float m_equilibriumPoint[6];
+	float m_springStiffness[6];
+	float m_springDamping[6];
 };
 
 struct btGeneric6DofSpringConstraintDoubleData2
 {
-	btGeneric6DofConstraintDoubleData2	m_6dofData;
-	
-	int			m_springEnabled[6];
-	double		m_equilibriumPoint[6];
-	double		m_springStiffness[6];
-	double		m_springDamping[6];
-};
+	btGeneric6DofConstraintDoubleData2 m_6dofData;
 
+	int m_springEnabled[6];
+	double m_equilibriumPoint[6];
+	double m_springStiffness[6];
+	double m_springDamping[6];
+};
 
-SIMD_FORCE_INLINE	int	btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btGeneric6DofSpringConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(btGeneric6DofSpringConstraintData2);
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btGeneric6DofSpringConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char* btGeneric6DofSpringConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 	btGeneric6DofSpringConstraintData2* dof = (btGeneric6DofSpringConstraintData2*)dataBuffer;
-	btGeneric6DofConstraint::serialize(&dof->m_6dofData,serializer);
+	btGeneric6DofConstraint::serialize(&dof->m_6dofData, serializer);
 
 	int i;
-	for (i=0;i<6;i++)
+	for (i = 0; i < 6; i++)
 	{
 		dof->m_equilibriumPoint[i] = m_equilibriumPoint[i];
 		dof->m_springDamping[i] = m_springDamping[i];
-		dof->m_springEnabled[i] = m_springEnabled[i]? 1 : 0;
+		dof->m_springEnabled[i] = m_springEnabled[i] ? 1 : 0;
 		dof->m_springStiffness[i] = m_springStiffness[i];
 	}
 	return btGeneric6DofSpringConstraintDataName;
 }
 
-#endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
-
+#endif  // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp
index 4be2aabe4d3..6507e1a0a74 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp
@@ -13,54 +13,49 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #include "btHinge2Constraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
 
-
-
 // constructor
 // anchor, axis1 and axis2 are in world coordinate system
 // axis1 must be orthogonal to axis2
 btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
-: btGeneric6DofSpring2Constraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(),RO_XYZ),
- m_anchor(anchor),
- m_axis1(axis1),
- m_axis2(axis2)
+	: btGeneric6DofSpring2Constraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), RO_XYZ),
+	  m_anchor(anchor),
+	  m_axis1(axis1),
+	  m_axis2(axis2)
 {
 	// build frame basis
 	// 6DOF constraint uses Euler angles and to define limits
 	// it is assumed that rotational order is :
 	// Z - first, allowed limits are (-PI,PI);
-	// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number 
+	// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
 	// used to prevent constraint from instability on poles;
 	// new position of X, allowed limits are (-PI,PI);
 	// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
 	// Build the frame in world coordinate system first
 	btVector3 zAxis = axis1.normalize();
 	btVector3 xAxis = axis2.normalize();
-	btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system
+	btVector3 yAxis = zAxis.cross(xAxis);  // we want right coordinate system
 	btTransform frameInW;
 	frameInW.setIdentity();
-	frameInW.getBasis().setValue(	xAxis[0], yAxis[0], zAxis[0],	
-									xAxis[1], yAxis[1], zAxis[1],
-									xAxis[2], yAxis[2], zAxis[2]);
+	frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
+								 xAxis[1], yAxis[1], zAxis[1],
+								 xAxis[2], yAxis[2], zAxis[2]);
 	frameInW.setOrigin(anchor);
 	// now get constraint frame in local coordinate systems
 	m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
 	m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
 	// sei limits
 	setLinearLowerLimit(btVector3(0.f, 0.f, -1.f));
-	setLinearUpperLimit(btVector3(0.f, 0.f,  1.f));
+	setLinearUpperLimit(btVector3(0.f, 0.f, 1.f));
 	// like front wheels of a car
-	setAngularLowerLimit(btVector3(1.f,  0.f, -SIMD_HALF_PI * 0.5f)); 
-	setAngularUpperLimit(btVector3(-1.f, 0.f,  SIMD_HALF_PI * 0.5f));
+	setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f));
+	setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f));
 	// enable suspension
 	enableSpring(2, true);
 	setStiffness(2, SIMD_PI * SIMD_PI * 4.f);
 	setDamping(2, 0.01f);
 	setEquilibriumPoint();
 }
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h
index 06a8e3ecd14..95f604a8906 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h
@@ -16,32 +16,30 @@ subject to the following restrictions:
 #ifndef BT_HINGE2_CONSTRAINT_H
 #define BT_HINGE2_CONSTRAINT_H
 
-
-
 #include "LinearMath/btVector3.h"
 #include "btTypedConstraint.h"
 #include "btGeneric6DofSpring2Constraint.h"
 
-
-
 // Constraint similar to ODE Hinge2 Joint
 // has 3 degrees of frredom:
 // 2 rotational degrees of freedom, similar to Euler rotations around Z (axis 1) and X (axis 2)
 // 1 translational (along axis Z) with suspension spring
 
-ATTRIBUTE_ALIGNED16(class) btHinge2Constraint : public btGeneric6DofSpring2Constraint
+ATTRIBUTE_ALIGNED16(class)
+btHinge2Constraint : public btGeneric6DofSpring2Constraint
 {
 protected:
-	btVector3	m_anchor;
-	btVector3	m_axis1;
-	btVector3	m_axis2;
+	btVector3 m_anchor;
+	btVector3 m_axis1;
+	btVector3 m_axis2;
+
 public:
-		BT_DECLARE_ALIGNED_ALLOCATOR();
-		
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	// constructor
 	// anchor, axis1 and axis2 are in world coordinate system
 	// axis1 must be orthogonal to axis2
-    btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2);
+	btHinge2Constraint(btRigidBody & rbA, btRigidBody & rbB, btVector3 & anchor, btVector3 & axis1, btVector3 & axis2);
 	// access
 	const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
 	const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
@@ -51,10 +49,7 @@ public:
 	btScalar getAngle2() { return getAngle(0); }
 	// limits
 	void setUpperLimit(btScalar ang1max) { setAngularUpperLimit(btVector3(-1.f, 0.f, ang1max)); }
-	void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3( 1.f, 0.f, ang1min)); }
+	void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3(1.f, 0.f, ang1min)); }
 };
 
-
-
-#endif // BT_HINGE2_CONSTRAINT_H
-
+#endif  // BT_HINGE2_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
index 76a1509471e..aa6f69000d2 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btHingeConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
@@ -21,8 +20,6 @@ subject to the following restrictions:
 #include <new>
 #include "btSolverBody.h"
 
-
-
 //#define HINGE_USE_OBSOLETE_SOLVER false
 #define HINGE_USE_OBSOLETE_SOLVER false
 
@@ -30,59 +27,60 @@ subject to the following restrictions:
 
 #ifndef __SPU__
 
-
-
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
-									 const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
-									 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& pivotInA, const btVector3& pivotInB,
+									 const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA)
+	: btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA, rbB),
 #ifdef _BT_USE_CENTER_LIMIT_
-									 m_limit(),
+	  m_limit(),
 #endif
-									 m_angularOnly(false),
-									 m_enableAngularMotor(false),
-									 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
-									 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
-									 m_useReferenceFrameA(useReferenceFrameA),
-									 m_flags(0),
-									 m_normalCFM(0),
-									 m_normalERP(0),
-									 m_stopCFM(0),
-									 m_stopERP(0)
+	  m_angularOnly(false),
+	  m_enableAngularMotor(false),
+	  m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+	  m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+	  m_useReferenceFrameA(useReferenceFrameA),
+	  m_flags(0),
+	  m_normalCFM(0),
+	  m_normalERP(0),
+	  m_stopCFM(0),
+	  m_stopERP(0)
 {
 	m_rbAFrame.getOrigin() = pivotInA;
-	
+
 	// since no frame is given, assume this to be zero angle and just pick rb transform axis
 	btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
 
 	btVector3 rbAxisA2;
 	btScalar projection = axisInA.dot(rbAxisA1);
-	if (projection >= 1.0f - SIMD_EPSILON) {
+	if (projection >= 1.0f - SIMD_EPSILON)
+	{
 		rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
 		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
-	} else if (projection <= -1.0f + SIMD_EPSILON) {
+	}
+	else if (projection <= -1.0f + SIMD_EPSILON)
+	{
 		rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
-		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);      
-	} else {
+		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
+	}
+	else
+	{
 		rbAxisA2 = axisInA.cross(rbAxisA1);
 		rbAxisA1 = rbAxisA2.cross(axisInA);
 	}
 
-	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
-									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
-									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+	m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(),
+								   rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(),
+								   rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ());
+
+	btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB);
+	btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1);
+	btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
 
-	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
-	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
-	btVector3 rbAxisB2 =  axisInB.cross(rbAxisB1);	
-	
 	m_rbBFrame.getOrigin() = pivotInB;
-	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
-									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
-									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
-	
-#ifndef	_BT_USE_CENTER_LIMIT_
+	m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(),
+								   rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(),
+								   rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ());
+
+#ifndef _BT_USE_CENTER_LIMIT_
 	//start with free
 	m_lowerLimit = btScalar(1.0f);
 	m_upperLimit = btScalar(-1.0f);
@@ -94,47 +92,44 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
 	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }
 
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
-:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA)
+	: btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),
 #ifdef _BT_USE_CENTER_LIMIT_
-m_limit(),
+	  m_limit(),
 #endif
-m_angularOnly(false), m_enableAngularMotor(false), 
-m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
-m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
-m_useReferenceFrameA(useReferenceFrameA),
-m_flags(0),
-m_normalCFM(0),
-m_normalERP(0),
-m_stopCFM(0),
-m_stopERP(0)
+	  m_angularOnly(false),
+	  m_enableAngularMotor(false),
+	  m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+	  m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+	  m_useReferenceFrameA(useReferenceFrameA),
+	  m_flags(0),
+	  m_normalCFM(0),
+	  m_normalERP(0),
+	  m_stopCFM(0),
+	  m_stopERP(0)
 {
-
 	// since no frame is given, assume this to be zero angle and just pick rb transform axis
 	// fixed axis in worldspace
 	btVector3 rbAxisA1, rbAxisA2;
 	btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
 
 	m_rbAFrame.getOrigin() = pivotInA;
-	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
-									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
-									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+	m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(),
+								   rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(),
+								   rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ());
 
 	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
 
-	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
-	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
+	btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB);
+	btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1);
 	btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
 
-
 	m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
-	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
-									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
-									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
-	
-#ifndef	_BT_USE_CENTER_LIMIT_
+	m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(),
+								   rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(),
+								   rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ());
+
+#ifndef _BT_USE_CENTER_LIMIT_
 	//start with free
 	m_lowerLimit = btScalar(1.0f);
 	m_upperLimit = btScalar(-1.0f);
@@ -146,26 +141,24 @@ m_stopERP(0)
 	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }
 
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
-								     const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
-:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, btRigidBody& rbB,
+									 const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
+	: btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA, rbB), m_rbAFrame(rbAFrame), m_rbBFrame(rbBFrame),
 #ifdef _BT_USE_CENTER_LIMIT_
-m_limit(),
+	  m_limit(),
 #endif
-m_angularOnly(false),
-m_enableAngularMotor(false),
-m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
-m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
-m_useReferenceFrameA(useReferenceFrameA),
-m_flags(0),
-m_normalCFM(0),
-m_normalERP(0),
-m_stopCFM(0),
-m_stopERP(0)
+	  m_angularOnly(false),
+	  m_enableAngularMotor(false),
+	  m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+	  m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+	  m_useReferenceFrameA(useReferenceFrameA),
+	  m_flags(0),
+	  m_normalCFM(0),
+	  m_normalERP(0),
+	  m_stopCFM(0),
+	  m_stopERP(0)
 {
-#ifndef	_BT_USE_CENTER_LIMIT_
+#ifndef _BT_USE_CENTER_LIMIT_
 	//start with free
 	m_lowerLimit = btScalar(1.0f);
 	m_upperLimit = btScalar(-1.0f);
@@ -175,30 +168,28 @@ m_stopERP(0)
 	m_solveLimit = false;
 #endif
 	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
-}			
-
-
+}
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
-:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
+	: btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_rbAFrame(rbAFrame), m_rbBFrame(rbAFrame),
 #ifdef _BT_USE_CENTER_LIMIT_
-m_limit(),
+	  m_limit(),
 #endif
-m_angularOnly(false),
-m_enableAngularMotor(false),
-m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
-m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
-m_useReferenceFrameA(useReferenceFrameA),
-m_flags(0),
-m_normalCFM(0),
-m_normalERP(0),
-m_stopCFM(0),
-m_stopERP(0)
+	  m_angularOnly(false),
+	  m_enableAngularMotor(false),
+	  m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+	  m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+	  m_useReferenceFrameA(useReferenceFrameA),
+	  m_flags(0),
+	  m_normalCFM(0),
+	  m_normalERP(0),
+	  m_stopCFM(0),
+	  m_stopERP(0)
 {
 	///not providing rigidbody B means implicitly using worldspace for body B
 
 	m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
-#ifndef	_BT_USE_CENTER_LIMIT_
+#ifndef _BT_USE_CENTER_LIMIT_
 	//start with free
 	m_lowerLimit = btScalar(1.0f);
 	m_upperLimit = btScalar(-1.0f);
@@ -210,9 +201,7 @@ m_stopERP(0)
 	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }
 
-
-
-void	btHingeConstraint::buildJacobian()
+void btHingeConstraint::buildJacobian()
 {
 	if (m_useSolveConstraintObsolete)
 	{
@@ -221,8 +210,8 @@ void	btHingeConstraint::buildJacobian()
 
 		if (!m_angularOnly)
 		{
-			btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-			btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+			btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
+			btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
 			btVector3 relPos = pivotBInW - pivotAInW;
 
 			btVector3 normal[3];
@@ -232,23 +221,23 @@ void	btHingeConstraint::buildJacobian()
 			}
 			else
 			{
-				normal[0].setValue(btScalar(1.0),0,0);
+				normal[0].setValue(btScalar(1.0), 0, 0);
 			}
 
 			btPlaneSpace1(normal[0], normal[1], normal[2]);
 
-			for (int i=0;i<3;i++)
+			for (int i = 0; i < 3; i++)
 			{
 				new (&m_jac[i]) btJacobianEntry(
-				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-				pivotAInW - m_rbA.getCenterOfMassPosition(),
-				pivotBInW - m_rbB.getCenterOfMassPosition(),
-				normal[i],
-				m_rbA.getInvInertiaDiagLocal(),
-				m_rbA.getInvMass(),
-				m_rbB.getInvInertiaDiagLocal(),
-				m_rbB.getInvMass());
+					m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+					m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+					pivotAInW - m_rbA.getCenterOfMassPosition(),
+					pivotBInW - m_rbB.getCenterOfMassPosition(),
+					normal[i],
+					m_rbA.getInvInertiaDiagLocal(),
+					m_rbA.getInvMass(),
+					m_rbB.getInvInertiaDiagLocal(),
+					m_rbB.getInvMass());
 			}
 		}
 
@@ -258,60 +247,55 @@ void	btHingeConstraint::buildJacobian()
 		//this is unused for now, it's a todo
 		btVector3 jointAxis0local;
 		btVector3 jointAxis1local;
-		
-		btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
+
+		btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2), jointAxis0local, jointAxis1local);
 
 		btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
 		btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
 		btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
-			
-		new (&m_jacAng[0])	btJacobianEntry(jointAxis0,
-			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-			m_rbA.getInvInertiaDiagLocal(),
-			m_rbB.getInvInertiaDiagLocal());
-
-		new (&m_jacAng[1])	btJacobianEntry(jointAxis1,
-			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-			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());
-
-			// clear accumulator
-			m_accLimitImpulse = btScalar(0.);
-
-			// test angular limit
-			testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
 
-		//Compute K = J*W*J' for hinge axis
-		btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
-		m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
-							 getRigidBodyB().computeAngularImpulseDenominator(axisA));
+		new (&m_jacAng[0]) btJacobianEntry(jointAxis0,
+										   m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+										   m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+										   m_rbA.getInvInertiaDiagLocal(),
+										   m_rbB.getInvInertiaDiagLocal());
 
-	}
-}
+		new (&m_jacAng[1]) btJacobianEntry(jointAxis1,
+										   m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+										   m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+										   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());
 
-#endif //__SPU__
+		// clear accumulator
+		m_accLimitImpulse = btScalar(0.);
 
+		// test angular limit
+		testLimit(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 
-static inline btScalar btNormalizeAnglePositive(btScalar angle)
-{
-  return btFmod(btFmod(angle, btScalar(2.0*SIMD_PI)) + btScalar(2.0*SIMD_PI), btScalar(2.0*SIMD_PI));
+		//Compute K = J*W*J' for hinge axis
+		btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+		m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
+						   getRigidBodyB().computeAngularImpulseDenominator(axisA));
+	}
 }
 
+#endif  //__SPU__
 
+static inline btScalar btNormalizeAnglePositive(btScalar angle)
+{
+	return btFmod(btFmod(angle, btScalar(2.0 * SIMD_PI)) + btScalar(2.0 * SIMD_PI), btScalar(2.0 * SIMD_PI));
+}
 
 static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
 {
 	btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) -
-	btNormalizeAnglePositive(accAngle)));
+																btNormalizeAnglePositive(accAngle)));
 	return result;
 }
 
@@ -320,41 +304,36 @@ static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
 	btScalar tol(0.3);
 	btScalar result = btShortestAngularDistance(accAngle, curAngle);
 
-	  if (btFabs(result) > tol)
+	if (btFabs(result) > tol)
 		return curAngle;
-	  else
+	else
 		return accAngle + result;
 
 	return curAngle;
 }
 
-
 btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle()
 {
 	btScalar hingeAngle = getHingeAngle();
-	m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,hingeAngle);
+	m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle, hingeAngle);
 	return m_accumulatedAngle;
 }
-void	btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle)
+void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle)
 {
-	m_accumulatedAngle  = accAngle;
+	m_accumulatedAngle = accAngle;
 }
 
 void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info)
 {
 	//update m_accumulatedAngle
 	btScalar curHingeAngle = getHingeAngle();
-	m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,curHingeAngle);
+	m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle, curHingeAngle);
 
 	btHingeConstraint::getInfo1(info);
-	
 }
 
-
 void btHingeConstraint::getInfo1(btConstraintInfo1* info)
 {
-
-
 	if (m_useSolveConstraintObsolete)
 	{
 		info->m_numConstraintRows = 0;
@@ -362,17 +341,16 @@ void btHingeConstraint::getInfo1(btConstraintInfo1* info)
 	}
 	else
 	{
-		info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
-		info->nub = 1; 
+		info->m_numConstraintRows = 5;  // Fixed 3 linear + 2 angular
+		info->nub = 1;
 		//always add the row, to avoid computation (data is not available yet)
 		//prepare constraint
-		testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
-		if(getSolveLimit() || getEnableAngularMotor())
+		testLimit(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
+		if (getSolveLimit() || getEnableAngularMotor())
 		{
-			info->m_numConstraintRows++; // limit 3rd anguar as well
-			info->nub--; 
+			info->m_numConstraintRows++;  // limit 3rd anguar as well
+			info->nub--;
 		}
-
 	}
 }
 
@@ -386,41 +364,38 @@ void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
 	else
 	{
 		//always add the 'limit' row, to avoid computation (data is not available yet)
-		info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular
-		info->nub = 0; 
+		info->m_numConstraintRows = 6;  // Fixed 3 linear + 2 angular
+		info->nub = 0;
 	}
 }
 
-void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
+void btHingeConstraint::getInfo2(btConstraintInfo2* info)
 {
-	if(m_useOffsetForConstraintFrame)
+	if (m_useOffsetForConstraintFrame)
 	{
-		getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
+		getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity());
 	}
 	else
 	{
-		getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
+		getInfo2Internal(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity());
 	}
 }
 
-
-void	btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+void btHingeConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
 {
 	///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now
-	testLimit(transA,transB);
+	testLimit(transA, transB);
 
-	getInfo2Internal(info,transA,transB,angVelA,angVelB);
+	getInfo2Internal(info, transA, transB, angVelA, angVelB);
 }
 
-
-void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
 {
-
 	btAssert(!m_useSolveConstraintObsolete);
 	int i, skip = info->rowskip;
 	// transforms in world space
-	btTransform trA = transA*m_rbAFrame;
-	btTransform trB = transB*m_rbBFrame;
+	btTransform trA = transA * m_rbAFrame;
+	btTransform trB = transB * m_rbBFrame;
 	// pivot point
 	btVector3 pivotAInW = trA.getOrigin();
 	btVector3 pivotBInW = trB.getOrigin();
@@ -448,7 +423,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 			info->m_constraintError[i*skip]=0.f;
 		}
 	}
-#endif //#if 0
+#endif  //#if 0
 	// linear (all fixed)
 
 	if (!m_angularOnly)
@@ -460,10 +435,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 		info->m_J2linearAxis[0] = -1;
 		info->m_J2linearAxis[skip + 1] = -1;
 		info->m_J2linearAxis[2 * skip + 2] = -1;
-	}	
-
-
-
+	}
 
 	btVector3 a1 = pivotAInW - transA.getOrigin();
 	{
@@ -471,22 +443,22 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
 		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
 		btVector3 a1neg = -a1;
-		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
 	btVector3 a2 = pivotBInW - transB.getOrigin();
 	{
 		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
 		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
 		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
-		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
 	// linear RHS
 	btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
 
-    btScalar k = info->fps * normalErp;
+	btScalar k = info->fps * normalErp;
 	if (!m_angularOnly)
 	{
-		for(i = 0; i < 3; i++)
+		for (i = 0; i < 3; i++)
 		{
 			info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
 		}
@@ -504,9 +476,9 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 	// get 2 orthos to hinge axis (X, Y)
 	btVector3 p = trA.getBasis().getColumn(0);
 	btVector3 q = trA.getBasis().getColumn(1);
-	// set the two hinge angular rows 
-    int s3 = 3 * info->rowskip;
-    int s4 = 4 * info->rowskip;
+	// set the two hinge angular rows
+	int s3 = 3 * info->rowskip;
+	int s4 = 4 * info->rowskip;
 
 	info->m_J1angularAxis[s3 + 0] = p[0];
 	info->m_J1angularAxis[s3 + 1] = p[1];
@@ -521,185 +493,172 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 	info->m_J2angularAxis[s4 + 0] = -q[0];
 	info->m_J2angularAxis[s4 + 1] = -q[1];
 	info->m_J2angularAxis[s4 + 2] = -q[2];
-    // compute the right hand side of the constraint equation. set relative
-    // body velocities along p and q to bring the hinge back into alignment.
-    // if ax1,ax2 are the unit length hinge axes as computed from body1 and
-    // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
-    // if `theta' is the angle between ax1 and ax2, we need an angular velocity
-    // along u to cover angle erp*theta in one step :
-    //   |angular_velocity| = angle/time = erp*theta / stepsize
-    //                      = (erp*fps) * theta
-    //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
-    //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
-    // ...as ax1 and ax2 are unit length. if theta is smallish,
-    // theta ~= sin(theta), so
-    //    angular_velocity  = (erp*fps) * (ax1 x ax2)
-    // ax1 x ax2 is in the plane space of ax1, so we project the angular
-    // velocity to p and q to find the right hand side.
-    btVector3 ax2 = trB.getBasis().getColumn(2);
+	// compute the right hand side of the constraint equation. set relative
+	// body velocities along p and q to bring the hinge back into alignment.
+	// if ax1,ax2 are the unit length hinge axes as computed from body1 and
+	// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
+	// if `theta' is the angle between ax1 and ax2, we need an angular velocity
+	// along u to cover angle erp*theta in one step :
+	//   |angular_velocity| = angle/time = erp*theta / stepsize
+	//                      = (erp*fps) * theta
+	//    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+	//                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+	// ...as ax1 and ax2 are unit length. if theta is smallish,
+	// theta ~= sin(theta), so
+	//    angular_velocity  = (erp*fps) * (ax1 x ax2)
+	// ax1 x ax2 is in the plane space of ax1, so we project the angular
+	// velocity to p and q to find the right hand side.
+	btVector3 ax2 = trB.getBasis().getColumn(2);
 	btVector3 u = ax1.cross(ax2);
 	info->m_constraintError[s3] = k * u.dot(p);
 	info->m_constraintError[s4] = k * u.dot(q);
 	// check angular limits
-	int nrow = 4; // last filled row
+	int nrow = 4;  // last filled row
 	int srow;
 	btScalar limit_err = btScalar(0.0);
 	int limit = 0;
-	if(getSolveLimit())
+	if (getSolveLimit())
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
-	limit_err = m_limit.getCorrection() * m_referenceSign;
+#ifdef _BT_USE_CENTER_LIMIT_
+		limit_err = m_limit.getCorrection() * m_referenceSign;
 #else
-	limit_err = m_correction * m_referenceSign;
+		limit_err = m_correction * m_referenceSign;
 #endif
-	limit = (limit_err > btScalar(0.0)) ? 1 : 2;
-
+		limit = (limit_err > btScalar(0.0)) ? 1 : 2;
 	}
 	// if the hinge has joint limits or motor, add in the extra row
-	int powered = 0;
-	if(getEnableAngularMotor())
-	{
-		powered = 1;
-	}
-	if(limit || powered) 
+	bool powered = getEnableAngularMotor();
+	if (limit || powered)
 	{
 		nrow++;
 		srow = nrow * info->rowskip;
-		info->m_J1angularAxis[srow+0] = ax1[0];
-		info->m_J1angularAxis[srow+1] = ax1[1];
-		info->m_J1angularAxis[srow+2] = ax1[2];
+		info->m_J1angularAxis[srow + 0] = ax1[0];
+		info->m_J1angularAxis[srow + 1] = ax1[1];
+		info->m_J1angularAxis[srow + 2] = ax1[2];
 
-		info->m_J2angularAxis[srow+0] = -ax1[0];
-		info->m_J2angularAxis[srow+1] = -ax1[1];
-		info->m_J2angularAxis[srow+2] = -ax1[2];
+		info->m_J2angularAxis[srow + 0] = -ax1[0];
+		info->m_J2angularAxis[srow + 1] = -ax1[1];
+		info->m_J2angularAxis[srow + 2] = -ax1[2];
 
 		btScalar lostop = getLowerLimit();
 		btScalar histop = getUpperLimit();
-		if(limit && (lostop == histop))
+		if (limit && (lostop == histop))
 		{  // the joint motor is ineffective
-			powered = 0;
+			powered = false;
 		}
 		info->m_constraintError[srow] = btScalar(0.0f);
 		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
-		if(powered)
+		if (powered)
 		{
-			if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
+			if (m_flags & BT_HINGE_FLAGS_CFM_NORM)
 			{
 				info->cfm[srow] = m_normalCFM;
 			}
 			btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
 			info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
-			info->m_lowerLimit[srow] = - m_maxMotorImpulse;
-			info->m_upperLimit[srow] =   m_maxMotorImpulse;
+			info->m_lowerLimit[srow] = -m_maxMotorImpulse;
+			info->m_upperLimit[srow] = m_maxMotorImpulse;
 		}
-		if(limit)
+		if (limit)
 		{
 			k = info->fps * currERP;
 			info->m_constraintError[srow] += k * limit_err;
-			if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
+			if (m_flags & BT_HINGE_FLAGS_CFM_STOP)
 			{
 				info->cfm[srow] = m_stopCFM;
 			}
-			if(lostop == histop) 
+			if (lostop == histop)
 			{
 				// limited low and high simultaneously
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
 			}
-			else if(limit == 1) 
-			{ // low limit
+			else if (limit == 1)
+			{  // low limit
 				info->m_lowerLimit[srow] = 0;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
 			}
-			else 
-			{ // high limit
+			else
+			{  // high limit
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = 0;
 			}
 			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 			btScalar bounce = m_limit.getRelaxationFactor();
 #else
 			btScalar bounce = m_relaxationFactor;
 #endif
-			if(bounce > btScalar(0.0))
+			if (bounce > btScalar(0.0))
 			{
 				btScalar vel = angVelA.dot(ax1);
 				vel -= angVelB.dot(ax1);
 				// only apply bounce if the velocity is incoming, and if the
 				// resulting c[] exceeds what we already have.
-				if(limit == 1)
-				{	// low limit
-					if(vel < 0)
+				if (limit == 1)
+				{  // low limit
+					if (vel < 0)
 					{
 						btScalar newc = -bounce * vel;
-						if(newc > info->m_constraintError[srow])
+						if (newc > info->m_constraintError[srow])
 						{
 							info->m_constraintError[srow] = newc;
 						}
 					}
 				}
 				else
-				{	// high limit - all those computations are reversed
-					if(vel > 0)
+				{  // high limit - all those computations are reversed
+					if (vel > 0)
 					{
 						btScalar newc = -bounce * vel;
-						if(newc < info->m_constraintError[srow])
+						if (newc < info->m_constraintError[srow])
 						{
 							info->m_constraintError[srow] = newc;
 						}
 					}
 				}
 			}
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 			info->m_constraintError[srow] *= m_limit.getBiasFactor();
 #else
 			info->m_constraintError[srow] *= m_biasFactor;
 #endif
-		} // if(limit)
-	} // if angular limit or powered
+		}  // if(limit)
+	}      // if angular limit or powered
 }
 
-
-void btHingeConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
+void btHingeConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
 {
 	m_rbAFrame = frameA;
 	m_rbBFrame = frameB;
 	buildJacobian();
 }
 
-
-void	btHingeConstraint::updateRHS(btScalar	timeStep)
+void btHingeConstraint::updateRHS(btScalar timeStep)
 {
 	(void)timeStep;
-
 }
 
-
-
-
 btScalar btHingeConstraint::getHingeAngle()
 {
-	return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	return getHingeAngle(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 }
 
-btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
+btScalar btHingeConstraint::getHingeAngle(const btTransform& transA, const btTransform& transB)
 {
-	const btVector3 refAxis0  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
-	const btVector3 refAxis1  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
+	const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
+	const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
 	const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
-//	btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+	//	btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
 	btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
 	return m_referenceSign * angle;
 }
 
-
-
-void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
+void btHingeConstraint::testLimit(const btTransform& transA, const btTransform& transB)
 {
 	// Compute limit information
-	m_hingeAngle = getHingeAngle(transA,transB);
-#ifdef	_BT_USE_CENTER_LIMIT_
+	m_hingeAngle = getHingeAngle(transA, transB);
+#ifdef _BT_USE_CENTER_LIMIT_
 	m_limit.test(m_hingeAngle);
 #else
 	m_correction = btScalar(0.);
@@ -713,7 +672,7 @@ void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& t
 			m_correction = (m_lowerLimit - m_hingeAngle);
 			m_limitSign = 1.0f;
 			m_solveLimit = true;
-		} 
+		}
 		else if (m_hingeAngle >= m_upperLimit)
 		{
 			m_correction = m_upperLimit - m_hingeAngle;
@@ -725,7 +684,6 @@ void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& t
 	return;
 }
 
-
 static btVector3 vHinge(0, 0, btScalar(1));
 
 void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
@@ -735,14 +693,15 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
 	qConstraint.normalize();
 
 	// extract "pure" hinge component
-	btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize();
+	btVector3 vNoHinge = quatRotate(qConstraint, vHinge);
+	vNoHinge.normalize();
 	btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
 	btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
 	qHinge.normalize();
 
 	// compute angular target, clamped to limits
 	btScalar targetAngle = qHinge.getAngle();
-	if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
+	if (targetAngle > SIMD_PI)  // long way around. flip quat and recalculate.
 	{
 		qHinge = -(qHinge);
 		targetAngle = qHinge.getAngle();
@@ -755,7 +714,7 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
 
 void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
 {
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 	m_limit.fit(targetAngle);
 #else
 	if (m_lowerLimit < m_upperLimit)
@@ -767,20 +726,18 @@ void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
 	}
 #endif
 	// compute angular velocity
-	btScalar curAngle  = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 	btScalar dAngle = targetAngle - curAngle;
 	m_motorTargetVelocity = dAngle / dt;
 }
 
-
-
-void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
 {
 	btAssert(!m_useSolveConstraintObsolete);
 	int i, s = info->rowskip;
 	// transforms in world space
-	btTransform trA = transA*m_rbAFrame;
-	btTransform trB = transB*m_rbBFrame;
+	btTransform trA = transA * m_rbAFrame;
+	btTransform trB = transB * m_rbBFrame;
 	// pivot point
 //	btVector3 pivotAInW = trA.getOrigin();
 //	btVector3 pivotBInW = trB.getOrigin();
@@ -793,11 +750,11 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 	bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
 	btScalar miS = miA + miB;
 	btScalar factA, factB;
-	if(miS > btScalar(0.f))
+	if (miS > btScalar(0.f))
 	{
 		factA = miB / miS;
 	}
-	else 
+	else
 	{
 		factA = btScalar(0.5f);
 	}
@@ -807,15 +764,21 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 	btVector3 ax1A = trA.getBasis().getColumn(2);
 	btVector3 ax1B = trB.getBasis().getColumn(2);
 	btVector3 ax1 = ax1A * factA + ax1B * factB;
+	if (ax1.length2()<SIMD_EPSILON)
+	{
+		factA=0.f;
+		factB=1.f;
+		ax1 = ax1A * factA + ax1B * factB;
+	}
 	ax1.normalize();
-	// fill first 3 rows 
+	// fill first 3 rows
 	// we want: velA + wA x relA == velB + wB x relB
 	btTransform bodyA_trans = transA;
 	btTransform bodyB_trans = transB;
 	int s0 = 0;
 	int s1 = s;
 	int s2 = s * 2;
-	int nrow = 2; // last filled row
+	int nrow = 2;  // last filled row
 	btVector3 tmpA, tmpB, relA, relB, p, q;
 	// get vector from bodyB to frameB in WCS
 	relB = trB.getOrigin() - bodyB_trans.getOrigin();
@@ -834,7 +797,7 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 	// now choose average ortho to hinge axis
 	p = orthoB * factA + orthoA * factB;
 	btScalar len2 = p.length2();
-	if(len2 > SIMD_EPSILON)
+	if (len2 > SIMD_EPSILON)
 	{
 		p /= btSqrt(len2);
 	}
@@ -847,44 +810,44 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 	// fill three rows
 	tmpA = relA.cross(p);
 	tmpB = relB.cross(p);
-    for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
-    for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
+	for (i = 0; i < 3; i++) info->m_J1angularAxis[s0 + i] = tmpA[i];
+	for (i = 0; i < 3; i++) info->m_J2angularAxis[s0 + i] = -tmpB[i];
 	tmpA = relA.cross(q);
 	tmpB = relB.cross(q);
-	if(hasStaticBody && getSolveLimit())
-	{ // to make constraint between static and dynamic objects more rigid
+	if (hasStaticBody && getSolveLimit())
+	{  // to make constraint between static and dynamic objects more rigid
 		// remove wA (or wB) from equation if angular limit is hit
 		tmpB *= factB;
 		tmpA *= factA;
 	}
-	for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
-    for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
+	for (i = 0; i < 3; i++) info->m_J1angularAxis[s1 + i] = tmpA[i];
+	for (i = 0; i < 3; i++) info->m_J2angularAxis[s1 + i] = -tmpB[i];
 	tmpA = relA.cross(ax1);
 	tmpB = relB.cross(ax1);
-	if(hasStaticBody)
-	{ // to make constraint between static and dynamic objects more rigid
+	if (hasStaticBody)
+	{  // to make constraint between static and dynamic objects more rigid
 		// remove wA (or wB) from equation
 		tmpB *= factB;
 		tmpA *= factA;
 	}
-	for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
-    for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
+	for (i = 0; i < 3; i++) info->m_J1angularAxis[s2 + i] = tmpA[i];
+	for (i = 0; i < 3; i++) info->m_J2angularAxis[s2 + i] = -tmpB[i];
 
-	btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp;
+	btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
 	btScalar k = info->fps * normalErp;
 
 	if (!m_angularOnly)
 	{
-		for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
-		for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
-		for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
+		for (i = 0; i < 3; i++) info->m_J1linearAxis[s0 + i] = p[i];
+		for (i = 0; i < 3; i++) info->m_J1linearAxis[s1 + i] = q[i];
+		for (i = 0; i < 3; i++) info->m_J1linearAxis[s2 + i] = ax1[i];
 
-		for (i=0; i<3; i++) info->m_J2linearAxis[s0+i] = -p[i];
-		for (i=0; i<3; i++) info->m_J2linearAxis[s1+i] = -q[i];
-		for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -ax1[i];
+		for (i = 0; i < 3; i++) info->m_J2linearAxis[s0 + i] = -p[i];
+		for (i = 0; i < 3; i++) info->m_J2linearAxis[s1 + i] = -q[i];
+		for (i = 0; i < 3; i++) info->m_J2linearAxis[s2 + i] = -ax1[i];
+
+		// compute three elements of right hand side
 
-	// compute three elements of right hand side
-	
 		btScalar rhs = k * p.dot(ofs);
 		info->m_constraintError[s0] = rhs;
 		rhs = k * q.dot(ofs);
@@ -929,150 +892,144 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 	//    angular_velocity  = (erp*fps) * (ax1 x ax2)
 	// ax1 x ax2 is in the plane space of ax1, so we project the angular
 	// velocity to p and q to find the right hand side.
-	k = info->fps * normalErp;//??
+	k = info->fps * normalErp;  //??
 
 	btVector3 u = ax1A.cross(ax1B);
 	info->m_constraintError[s3] = k * u.dot(p);
 	info->m_constraintError[s4] = k * u.dot(q);
 #endif
 	// check angular limits
-	nrow = 4; // last filled row
+	nrow = 4;  // last filled row
 	int srow;
 	btScalar limit_err = btScalar(0.0);
 	int limit = 0;
-	if(getSolveLimit())
+	if (getSolveLimit())
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
-	limit_err = m_limit.getCorrection() * m_referenceSign;
+#ifdef _BT_USE_CENTER_LIMIT_
+		limit_err = m_limit.getCorrection() * m_referenceSign;
 #else
-	limit_err = m_correction * m_referenceSign;
+		limit_err = m_correction * m_referenceSign;
 #endif
-	limit = (limit_err > btScalar(0.0)) ? 1 : 2;
-
+		limit = (limit_err > btScalar(0.0)) ? 1 : 2;
 	}
 	// if the hinge has joint limits or motor, add in the extra row
-	int powered = 0;
-	if(getEnableAngularMotor())
-	{
-		powered = 1;
-	}
-	if(limit || powered) 
+	bool powered = getEnableAngularMotor();
+	if (limit || powered)
 	{
 		nrow++;
 		srow = nrow * info->rowskip;
-		info->m_J1angularAxis[srow+0] = ax1[0];
-		info->m_J1angularAxis[srow+1] = ax1[1];
-		info->m_J1angularAxis[srow+2] = ax1[2];
+		info->m_J1angularAxis[srow + 0] = ax1[0];
+		info->m_J1angularAxis[srow + 1] = ax1[1];
+		info->m_J1angularAxis[srow + 2] = ax1[2];
 
-		info->m_J2angularAxis[srow+0] = -ax1[0];
-		info->m_J2angularAxis[srow+1] = -ax1[1];
-		info->m_J2angularAxis[srow+2] = -ax1[2];
+		info->m_J2angularAxis[srow + 0] = -ax1[0];
+		info->m_J2angularAxis[srow + 1] = -ax1[1];
+		info->m_J2angularAxis[srow + 2] = -ax1[2];
 
 		btScalar lostop = getLowerLimit();
 		btScalar histop = getUpperLimit();
-		if(limit && (lostop == histop))
+		if (limit && (lostop == histop))
 		{  // the joint motor is ineffective
-			powered = 0;
+			powered = false;
 		}
 		info->m_constraintError[srow] = btScalar(0.0f);
 		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
-		if(powered)
+		if (powered)
 		{
-			if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
+			if (m_flags & BT_HINGE_FLAGS_CFM_NORM)
 			{
 				info->cfm[srow] = m_normalCFM;
 			}
 			btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
 			info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
-			info->m_lowerLimit[srow] = - m_maxMotorImpulse;
-			info->m_upperLimit[srow] =   m_maxMotorImpulse;
+			info->m_lowerLimit[srow] = -m_maxMotorImpulse;
+			info->m_upperLimit[srow] = m_maxMotorImpulse;
 		}
-		if(limit)
+		if (limit)
 		{
 			k = info->fps * currERP;
 			info->m_constraintError[srow] += k * limit_err;
-			if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
+			if (m_flags & BT_HINGE_FLAGS_CFM_STOP)
 			{
 				info->cfm[srow] = m_stopCFM;
 			}
-			if(lostop == histop) 
+			if (lostop == histop)
 			{
 				// limited low and high simultaneously
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
 			}
-			else if(limit == 1) 
-			{ // low limit
+			else if (limit == 1)
+			{  // low limit
 				info->m_lowerLimit[srow] = 0;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
 			}
-			else 
-			{ // high limit
+			else
+			{  // high limit
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = 0;
 			}
 			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 			btScalar bounce = m_limit.getRelaxationFactor();
 #else
 			btScalar bounce = m_relaxationFactor;
 #endif
-			if(bounce > btScalar(0.0))
+			if (bounce > btScalar(0.0))
 			{
 				btScalar vel = angVelA.dot(ax1);
 				vel -= angVelB.dot(ax1);
 				// only apply bounce if the velocity is incoming, and if the
 				// resulting c[] exceeds what we already have.
-				if(limit == 1)
-				{	// low limit
-					if(vel < 0)
+				if (limit == 1)
+				{  // low limit
+					if (vel < 0)
 					{
 						btScalar newc = -bounce * vel;
-						if(newc > info->m_constraintError[srow])
+						if (newc > info->m_constraintError[srow])
 						{
 							info->m_constraintError[srow] = newc;
 						}
 					}
 				}
 				else
-				{	// high limit - all those computations are reversed
-					if(vel > 0)
+				{  // high limit - all those computations are reversed
+					if (vel > 0)
 					{
 						btScalar newc = -bounce * vel;
-						if(newc < info->m_constraintError[srow])
+						if (newc < info->m_constraintError[srow])
 						{
 							info->m_constraintError[srow] = newc;
 						}
 					}
 				}
 			}
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 			info->m_constraintError[srow] *= m_limit.getBiasFactor();
 #else
 			info->m_constraintError[srow] *= m_biasFactor;
 #endif
-		} // if(limit)
-	} // if angular limit or powered
+		}  // if(limit)
+	}      // if angular limit or powered
 }
 
-
-///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 ///If no axis is provided, it uses the default axis for this constraint.
 void btHingeConstraint::setParam(int num, btScalar value, int axis)
 {
-	if((axis == -1) || (axis == 5))
+	if ((axis == -1) || (axis == 5))
 	{
-		switch(num)
-		{	
-			case BT_CONSTRAINT_STOP_ERP :
+		switch (num)
+		{
+			case BT_CONSTRAINT_STOP_ERP:
 				m_stopERP = value;
 				m_flags |= BT_HINGE_FLAGS_ERP_STOP;
 				break;
-			case BT_CONSTRAINT_STOP_CFM :
+			case BT_CONSTRAINT_STOP_CFM:
 				m_stopCFM = value;
 				m_flags |= BT_HINGE_FLAGS_CFM_STOP;
 				break;
-			case BT_CONSTRAINT_CFM :
+			case BT_CONSTRAINT_CFM:
 				m_normalCFM = value;
 				m_flags |= BT_HINGE_FLAGS_CFM_NORM;
 				break;
@@ -1080,7 +1037,7 @@ void btHingeConstraint::setParam(int num, btScalar value, int axis)
 				m_normalERP = value;
 				m_flags |= BT_HINGE_FLAGS_ERP_NORM;
 				break;
-			default : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
@@ -1091,22 +1048,22 @@ void btHingeConstraint::setParam(int num, btScalar value, int axis)
 }
 
 ///return the local value of parameter
-btScalar btHingeConstraint::getParam(int num, int axis) const 
+btScalar btHingeConstraint::getParam(int num, int axis) const
 {
 	btScalar retVal = 0;
-	if((axis == -1) || (axis == 5))
+	if ((axis == -1) || (axis == 5))
 	{
-		switch(num)
-		{	
-			case BT_CONSTRAINT_STOP_ERP :
+		switch (num)
+		{
+			case BT_CONSTRAINT_STOP_ERP:
 				btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
 				retVal = m_stopERP;
 				break;
-			case BT_CONSTRAINT_STOP_CFM :
+			case BT_CONSTRAINT_STOP_CFM:
 				btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
 				retVal = m_stopCFM;
 				break;
-			case BT_CONSTRAINT_CFM :
+			case BT_CONSTRAINT_CFM:
 				btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
 				retVal = m_normalCFM;
 				break;
@@ -1114,7 +1071,7 @@ btScalar btHingeConstraint::getParam(int num, int axis) const
 				btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM);
 				retVal = m_normalERP;
 				break;
-			default : 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
@@ -1124,5 +1081,3 @@ btScalar btHingeConstraint::getParam(int num, int axis) const
 	}
 	return retVal;
 }
-
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
index f26e72105ba..c7509e30afb 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
@@ -20,7 +20,6 @@ subject to the following restrictions:
 
 #define _BT_USE_CENTER_LIMIT_ 1
 
-
 #include "LinearMath/btVector3.h"
 #include "btJacobianEntry.h"
 #include "btTypedConstraint.h"
@@ -28,14 +27,12 @@ subject to the following restrictions:
 class btRigidBody;
 
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btHingeConstraintData	btHingeConstraintDoubleData2 //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version
-#define btHingeConstraintDataName	"btHingeConstraintDoubleData2" 
+#define btHingeConstraintData btHingeConstraintDoubleData2  //rename to 2 for backwards compatibility, so we can still load the 'btHingeConstraintDoubleData' version
+#define btHingeConstraintDataName "btHingeConstraintDoubleData2"
 #else
-#define btHingeConstraintData	btHingeConstraintFloatData
-#define btHingeConstraintDataName	"btHingeConstraintFloatData"
-#endif //BT_USE_DOUBLE_PRECISION
-
-
+#define btHingeConstraintData btHingeConstraintFloatData
+#define btHingeConstraintDataName "btHingeConstraintFloatData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
 enum btHingeFlags
 {
@@ -45,89 +42,83 @@ enum btHingeFlags
 	BT_HINGE_FLAGS_ERP_NORM = 8
 };
 
-
 /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
 /// axis defines the orientation of the hinge axis
-ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class)
+btHingeConstraint : public btTypedConstraint
 {
 #ifdef IN_PARALLELL_SOLVER
 public:
 #endif
-	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
-	btJacobianEntry	m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
+	btJacobianEntry m_jac[3];     //3 orthogonal linear constraints
+	btJacobianEntry m_jacAng[3];  //2 orthogonal angular constraints+ 1 for limit/motor
 
-	btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTransform m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 	btTransform m_rbBFrame;
 
-	btScalar	m_motorTargetVelocity;
-	btScalar	m_maxMotorImpulse;
-
+	btScalar m_motorTargetVelocity;
+	btScalar m_maxMotorImpulse;
 
-#ifdef	_BT_USE_CENTER_LIMIT_
-	btAngularLimit	m_limit;
+#ifdef _BT_USE_CENTER_LIMIT_
+	btAngularLimit m_limit;
 #else
-	btScalar	m_lowerLimit;	
-	btScalar	m_upperLimit;	
-	btScalar	m_limitSign;
-	btScalar	m_correction;
+	btScalar m_lowerLimit;
+	btScalar m_upperLimit;
+	btScalar m_limitSign;
+	btScalar m_correction;
 
-	btScalar	m_limitSoftness; 
-	btScalar	m_biasFactor; 
-	btScalar	m_relaxationFactor; 
+	btScalar m_limitSoftness;
+	btScalar m_biasFactor;
+	btScalar m_relaxationFactor;
 
-	bool		m_solveLimit;
+	bool m_solveLimit;
 #endif
 
-	btScalar	m_kHinge;
-
+	btScalar m_kHinge;
 
-	btScalar	m_accLimitImpulse;
-	btScalar	m_hingeAngle;
-	btScalar	m_referenceSign;
+	btScalar m_accLimitImpulse;
+	btScalar m_hingeAngle;
+	btScalar m_referenceSign;
 
-	bool		m_angularOnly;
-	bool		m_enableAngularMotor;
-	bool		m_useSolveConstraintObsolete;
-	bool		m_useOffsetForConstraintFrame;
-	bool		m_useReferenceFrameA;
+	bool m_angularOnly;
+	bool m_enableAngularMotor;
+	bool m_useSolveConstraintObsolete;
+	bool m_useOffsetForConstraintFrame;
+	bool m_useReferenceFrameA;
 
-	btScalar	m_accMotorImpulse;
+	btScalar m_accMotorImpulse;
 
-	int			m_flags;
-	btScalar	m_normalCFM;
-	btScalar	m_normalERP;
-	btScalar	m_stopCFM;
-	btScalar	m_stopERP;
+	int m_flags;
+	btScalar m_normalCFM;
+	btScalar m_normalERP;
+	btScalar m_stopCFM;
+	btScalar m_stopERP;
 
-	
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
-	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false);
 
-	btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false);
-	
-	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
+	btHingeConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& pivotInA, const btVector3& pivotInB, const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA = false);
+
+	btHingeConstraint(btRigidBody & rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA = false);
 
-	btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
+	btHingeConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
 
+	btHingeConstraint(btRigidBody & rbA, const btTransform& rbAFrame, bool useReferenceFrameA = false);
 
-	virtual void	buildJacobian();
+	virtual void buildJacobian();
 
-	virtual void getInfo1 (btConstraintInfo1* info);
+	virtual void getInfo1(btConstraintInfo1 * info);
 
-	void getInfo1NonVirtual(btConstraintInfo1* info);
+	void getInfo1NonVirtual(btConstraintInfo1 * info);
 
-	virtual void getInfo2 (btConstraintInfo2* info);
+	virtual void getInfo2(btConstraintInfo2 * info);
 
-	void	getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
+	void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB);
 
-	void	getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
-	void	getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
-		
+	void getInfo2Internal(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB);
+	void getInfo2InternalUsingFrameOffset(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB);
 
-	void	updateRHS(btScalar	timeStep);
+	void updateRHS(btScalar timeStep);
 
 	const btRigidBody& getRigidBodyA() const
 	{
@@ -138,19 +129,19 @@ public:
 		return m_rbB;
 	}
 
-	btRigidBody& getRigidBodyA()	
-	{		
-		return m_rbA;	
-	}	
+	btRigidBody& getRigidBodyA()
+	{
+		return m_rbA;
+	}
 
-	btRigidBody& getRigidBodyB()	
-	{		
-		return m_rbB;	
+	btRigidBody& getRigidBodyB()
+	{
+		return m_rbB;
 	}
 
 	btTransform& getFrameOffsetA()
 	{
-	return m_rbAFrame;
+		return m_rbAFrame;
 	}
 
 	btTransform& getFrameOffsetB()
@@ -159,15 +150,15 @@ public:
 	}
 
 	void setFrames(const btTransform& frameA, const btTransform& frameB);
-	
-	void	setAngularOnly(bool angularOnly)
+
+	void setAngularOnly(bool angularOnly)
 	{
 		m_angularOnly = angularOnly;
 	}
 
-	void	enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse)
+	void enableAngularMotor(bool enableMotor, btScalar targetVelocity, btScalar maxMotorImpulse)
 	{
-		m_enableAngularMotor  = enableMotor;
+		m_enableAngularMotor = enableMotor;
 		m_motorTargetVelocity = targetVelocity;
 		m_maxMotorImpulse = maxMotorImpulse;
 	}
@@ -175,29 +166,28 @@ public:
 	// extra motor API, including ability to set a target rotation (as opposed to angular velocity)
 	// note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to
 	//       maintain a given angular target.
-	void enableMotor(bool enableMotor) 	{ m_enableAngularMotor = enableMotor; }
+	void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; }
 	void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
 	void setMotorTargetVelocity(btScalar motorTargetVelocity) { m_motorTargetVelocity = motorTargetVelocity; }
-	void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
+	void setMotorTarget(const btQuaternion& qAinB, btScalar dt);  // qAinB is rotation of body A wrt body B.
 	void setMotorTarget(btScalar targetAngle, btScalar dt);
 
-
-	void	setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+	void setLimit(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 		m_limit.set(low, high, _softness, _biasFactor, _relaxationFactor);
 #else
 		m_lowerLimit = btNormalizeAngle(low);
 		m_upperLimit = btNormalizeAngle(high);
-		m_limitSoftness =  _softness;
+		m_limitSoftness = _softness;
 		m_biasFactor = _biasFactor;
 		m_relaxationFactor = _relaxationFactor;
 #endif
 	}
-	
+
 	btScalar getLimitSoftness() const
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 		return m_limit.getSoftness();
 #else
 		return m_limitSoftness;
@@ -206,7 +196,7 @@ public:
 
 	btScalar getLimitBiasFactor() const
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 		return m_limit.getBiasFactor();
 #else
 		return m_biasFactor;
@@ -215,112 +205,110 @@ public:
 
 	btScalar getLimitRelaxationFactor() const
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 		return m_limit.getRelaxationFactor();
 #else
 		return m_relaxationFactor;
 #endif
 	}
 
-	void	setAxis(btVector3& axisInA)
+	void setAxis(btVector3 & axisInA)
 	{
 		btVector3 rbAxisA1, rbAxisA2;
 		btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
 		btVector3 pivotInA = m_rbAFrame.getOrigin();
-//		m_rbAFrame.getOrigin() = pivotInA;
-		m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
-										rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
-										rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+		//		m_rbAFrame.getOrigin() = pivotInA;
+		m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(),
+									   rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(),
+									   rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ());
 
 		btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA;
 
-		btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
-		btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
+		btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB);
+		btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1);
 		btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
 
 		m_rbBFrame.getOrigin() = m_rbB.getCenterOfMassTransform().inverse()(m_rbA.getCenterOfMassTransform()(pivotInA));
 
-		m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
-										rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
-										rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
+		m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(),
+									   rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(),
+									   rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ());
 		m_rbBFrame.getBasis() = m_rbB.getCenterOfMassTransform().getBasis().inverse() * m_rbBFrame.getBasis();
-
 	}
 
-    bool hasLimit() const {
-#ifdef  _BT_USE_CENTER_LIMIT_
-        return m_limit.getHalfRange() > 0;
+	bool hasLimit() const
+	{
+#ifdef _BT_USE_CENTER_LIMIT_
+		return m_limit.getHalfRange() > 0;
 #else
-        return m_lowerLimit <= m_upperLimit;
+		return m_lowerLimit <= m_upperLimit;
 #endif
-    }
+	}
 
-	btScalar	getLowerLimit() const
+	btScalar getLowerLimit() const
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
-	return m_limit.getLow();
+#ifdef _BT_USE_CENTER_LIMIT_
+		return m_limit.getLow();
 #else
-	return m_lowerLimit;
+		return m_lowerLimit;
 #endif
 	}
 
-	btScalar	getUpperLimit() const
+	btScalar getUpperLimit() const
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
-	return m_limit.getHigh();
-#else		
-	return m_upperLimit;
+#ifdef _BT_USE_CENTER_LIMIT_
+		return m_limit.getHigh();
+#else
+		return m_upperLimit;
 #endif
 	}
 
-
 	///The getHingeAngle gives the hinge angle in range [-PI,PI]
 	btScalar getHingeAngle();
 
-	btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
+	btScalar getHingeAngle(const btTransform& transA, const btTransform& transB);
 
-	void testLimit(const btTransform& transA,const btTransform& transB);
+	void testLimit(const btTransform& transA, const btTransform& transB);
 
-
-	const btTransform& getAFrame() const { return m_rbAFrame; };	
+	const btTransform& getAFrame() const { return m_rbAFrame; };
 	const btTransform& getBFrame() const { return m_rbBFrame; };
 
-	btTransform& getAFrame() { return m_rbAFrame; };	
+	btTransform& getAFrame() { return m_rbAFrame; };
 	btTransform& getBFrame() { return m_rbBFrame; };
 
 	inline int getSolveLimit()
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
-	return m_limit.isLimit();
+#ifdef _BT_USE_CENTER_LIMIT_
+		return m_limit.isLimit();
 #else
-	return m_solveLimit;
+		return m_solveLimit;
 #endif
 	}
 
 	inline btScalar getLimitSign()
 	{
-#ifdef	_BT_USE_CENTER_LIMIT_
-	return m_limit.getSign();
+#ifdef _BT_USE_CENTER_LIMIT_
+		return m_limit.getSign();
 #else
 		return m_limitSign;
 #endif
 	}
 
-	inline bool getAngularOnly() 
-	{ 
-		return m_angularOnly; 
+	inline bool getAngularOnly()
+	{
+		return m_angularOnly;
 	}
-	inline bool getEnableAngularMotor() 
-	{ 
-		return m_enableAngularMotor; 
+	inline bool getEnableAngularMotor()
+	{
+		return m_enableAngularMotor;
 	}
-	inline btScalar getMotorTargetVelosity() 
-	{ 
-		return m_motorTargetVelocity; 
+	inline btScalar getMotorTargetVelocity()
+	{
+		return m_motorTargetVelocity;
 	}
-	inline btScalar getMaxMotorImpulse() 
-	{ 
-		return m_maxMotorImpulse; 
+	inline btScalar getMaxMotorImpulse()
+	{
+		return m_maxMotorImpulse;
 	}
 	// access for UseFrameOffset
 	bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
@@ -329,143 +317,132 @@ public:
 	bool getUseReferenceFrameA() const { return m_useReferenceFrameA; }
 	void setUseReferenceFrameA(bool useReferenceFrameA) { m_useReferenceFrameA = useReferenceFrameA; }
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 	///If no axis is provided, it uses the default axis for this constraint.
-	virtual	void	setParam(int num, btScalar value, int axis = -1);
+	virtual void setParam(int num, btScalar value, int axis = -1);
 	///return the local value of parameter
-	virtual	btScalar getParam(int num, int axis = -1) const;
-	
-	virtual	int getFlags() const
+	virtual btScalar getParam(int num, int axis = -1) const;
+
+	virtual int getFlags() const
 	{
-  	    return m_flags;
+		return m_flags;
 	}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	virtual int calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
-
-
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
-
 //only for backward compatibility
 #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 ///this structure is not used, except for loading pre-2.82 .bullet files
-struct	btHingeConstraintDoubleData
+struct btHingeConstraintDoubleData
 {
-	btTypedConstraintData	m_typeConstraintData;
-	btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTypedConstraintData m_typeConstraintData;
+	btTransformDoubleData m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 	btTransformDoubleData m_rbBFrame;
-	int			m_useReferenceFrameA;
-	int			m_angularOnly;
-	int			m_enableAngularMotor;
-	float	m_motorTargetVelocity;
-	float	m_maxMotorImpulse;
-
-	float	m_lowerLimit;
-	float	m_upperLimit;
-	float	m_limitSoftness;
-	float	m_biasFactor;
-	float	m_relaxationFactor;
-
+	int m_useReferenceFrameA;
+	int m_angularOnly;
+	int m_enableAngularMotor;
+	float m_motorTargetVelocity;
+	float m_maxMotorImpulse;
+
+	float m_lowerLimit;
+	float m_upperLimit;
+	float m_limitSoftness;
+	float m_biasFactor;
+	float m_relaxationFactor;
 };
-#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+#endif  //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 
 ///The getAccumulatedHingeAngle returns the accumulated hinge angle, taking rotation across the -PI/PI boundary into account
-ATTRIBUTE_ALIGNED16(class) btHingeAccumulatedAngleConstraint : public btHingeConstraint
+ATTRIBUTE_ALIGNED16(class)
+btHingeAccumulatedAngleConstraint : public btHingeConstraint
 {
 protected:
-	btScalar	m_accumulatedAngle;
-public:
+	btScalar m_accumulatedAngle;
 
+public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
-	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false)
-	:btHingeConstraint(rbA,rbB,pivotInA,pivotInB, axisInA,axisInB, useReferenceFrameA )
+
+	btHingeAccumulatedAngleConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& pivotInA, const btVector3& pivotInB, const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA = false)
+		: btHingeConstraint(rbA, rbB, pivotInA, pivotInB, axisInA, axisInB, useReferenceFrameA)
 	{
-		m_accumulatedAngle=getHingeAngle();
+		m_accumulatedAngle = getHingeAngle();
 	}
 
-	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false)
-	:btHingeConstraint(rbA,pivotInA,axisInA, useReferenceFrameA)
+	btHingeAccumulatedAngleConstraint(btRigidBody & rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA = false)
+		: btHingeConstraint(rbA, pivotInA, axisInA, useReferenceFrameA)
 	{
-		m_accumulatedAngle=getHingeAngle();
+		m_accumulatedAngle = getHingeAngle();
 	}
-	
-	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false)
-	:btHingeConstraint(rbA,rbB, rbAFrame, rbBFrame, useReferenceFrameA )
+
+	btHingeAccumulatedAngleConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false)
+		: btHingeConstraint(rbA, rbB, rbAFrame, rbBFrame, useReferenceFrameA)
 	{
-		m_accumulatedAngle=getHingeAngle();
+		m_accumulatedAngle = getHingeAngle();
 	}
 
-	btHingeAccumulatedAngleConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false)
-	:btHingeConstraint(rbA,rbAFrame, useReferenceFrameA )
+	btHingeAccumulatedAngleConstraint(btRigidBody & rbA, const btTransform& rbAFrame, bool useReferenceFrameA = false)
+		: btHingeConstraint(rbA, rbAFrame, useReferenceFrameA)
 	{
-		m_accumulatedAngle=getHingeAngle();
+		m_accumulatedAngle = getHingeAngle();
 	}
 	btScalar getAccumulatedHingeAngle();
-	void	setAccumulatedHingeAngle(btScalar accAngle);
-	virtual void getInfo1 (btConstraintInfo1* info);
-
+	void setAccumulatedHingeAngle(btScalar accAngle);
+	virtual void getInfo1(btConstraintInfo1 * info);
 };
 
-struct	btHingeConstraintFloatData
+struct btHingeConstraintFloatData
 {
-	btTypedConstraintData	m_typeConstraintData;
-	btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTypedConstraintData m_typeConstraintData;
+	btTransformFloatData m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 	btTransformFloatData m_rbBFrame;
-	int			m_useReferenceFrameA;
-	int			m_angularOnly;
-	
-	int			m_enableAngularMotor;
-	float	m_motorTargetVelocity;
-	float	m_maxMotorImpulse;
-
-	float	m_lowerLimit;
-	float	m_upperLimit;
-	float	m_limitSoftness;
-	float	m_biasFactor;
-	float	m_relaxationFactor;
-
+	int m_useReferenceFrameA;
+	int m_angularOnly;
+
+	int m_enableAngularMotor;
+	float m_motorTargetVelocity;
+	float m_maxMotorImpulse;
+
+	float m_lowerLimit;
+	float m_upperLimit;
+	float m_limitSoftness;
+	float m_biasFactor;
+	float m_relaxationFactor;
 };
 
-
-
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btHingeConstraintDoubleData2
+struct btHingeConstraintDoubleData2
 {
-	btTypedConstraintDoubleData	m_typeConstraintData;
-	btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTypedConstraintDoubleData m_typeConstraintData;
+	btTransformDoubleData m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 	btTransformDoubleData m_rbBFrame;
-	int			m_useReferenceFrameA;
-	int			m_angularOnly;
-	int			m_enableAngularMotor;
-	double		m_motorTargetVelocity;
-	double		m_maxMotorImpulse;
-
-	double		m_lowerLimit;
-	double		m_upperLimit;
-	double		m_limitSoftness;
-	double		m_biasFactor;
-	double		m_relaxationFactor;
-	char	m_padding1[4];
-
+	int m_useReferenceFrameA;
+	int m_angularOnly;
+	int m_enableAngularMotor;
+	double m_motorTargetVelocity;
+	double m_maxMotorImpulse;
+
+	double m_lowerLimit;
+	double m_upperLimit;
+	double m_limitSoftness;
+	double m_biasFactor;
+	double m_relaxationFactor;
+	char m_padding1[4];
 };
 
-
-
-
-SIMD_FORCE_INLINE	int	btHingeConstraint::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(btHingeConstraintData);
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 	btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer;
-	btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer);
+	btTypedConstraint::serialize(&hingeData->m_typeConstraintData, serializer);
 
 	m_rbAFrame.serialize(hingeData->m_rbAFrame);
 	m_rbBFrame.serialize(hingeData->m_rbBFrame);
@@ -475,7 +452,7 @@ SIMD_FORCE_INLINE	const char*	btHingeConstraint::serialize(void* dataBuffer, btS
 	hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
 	hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
 	hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
-#ifdef	_BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
 	hingeData->m_lowerLimit = float(m_limit.getLow());
 	hingeData->m_upperLimit = float(m_limit.getHigh());
 	hingeData->m_limitSoftness = float(m_limit.getSoftness());
@@ -489,7 +466,15 @@ SIMD_FORCE_INLINE	const char*	btHingeConstraint::serialize(void* dataBuffer, btS
 	hingeData->m_relaxationFactor = float(m_relaxationFactor);
 #endif
 
+	// Fill padding with zeros to appease msan.
+#ifdef BT_USE_DOUBLE_PRECISION
+	hingeData->m_padding1[0] = 0;
+	hingeData->m_padding1[1] = 0;
+	hingeData->m_padding1[2] = 0;
+	hingeData->m_padding1[3] = 0;
+#endif
+
 	return btHingeConstraintDataName;
 }
 
-#endif //BT_HINGECONSTRAINT_H
+#endif  //BT_HINGECONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
index 125580d1998..438456fe512 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
@@ -18,7 +18,6 @@ subject to the following restrictions:
 
 #include "LinearMath/btMatrix3x3.h"
 
-
 //notes:
 // Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
 // which makes the btJacobianEntry memory layout 16 bytes
@@ -27,25 +26,26 @@ subject to the following restrictions:
 /// Jacobian entry is an abstraction that allows to describe constraints
 /// it can be used in combination with a constraint solver
 /// Can be used to relate the effect of an impulse to the constraint error
-ATTRIBUTE_ALIGNED16(class) btJacobianEntry
+ATTRIBUTE_ALIGNED16(class)
+btJacobianEntry
 {
 public:
-	btJacobianEntry() {};
+	btJacobianEntry(){};
 	//constraint between two different rigidbodies
 	btJacobianEntry(
 		const btMatrix3x3& world2A,
 		const btMatrix3x3& world2B,
-		const btVector3& rel_pos1,const btVector3& rel_pos2,
+		const btVector3& rel_pos1, const btVector3& rel_pos2,
 		const btVector3& jointAxis,
-		const btVector3& inertiaInvA, 
+		const btVector3& inertiaInvA,
 		const btScalar massInvA,
 		const btVector3& inertiaInvB,
 		const btScalar massInvB)
-		:m_linearJointAxis(jointAxis)
+		: m_linearJointAxis(jointAxis)
 	{
-		m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
-		m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
-		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_aJ = world2A * (rel_pos1.cross(m_linearJointAxis));
+		m_bJ = world2B * (rel_pos2.cross(-m_linearJointAxis));
+		m_0MinvJt = inertiaInvA * m_aJ;
 		m_1MinvJt = inertiaInvB * m_bJ;
 		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
 
@@ -54,33 +54,31 @@ public:
 
 	//angular constraint between two different rigidbodies
 	btJacobianEntry(const btVector3& jointAxis,
-		const btMatrix3x3& world2A,
-		const btMatrix3x3& world2B,
-		const btVector3& inertiaInvA,
-		const btVector3& inertiaInvB)
-		:m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
+					const btMatrix3x3& world2A,
+					const btMatrix3x3& world2B,
+					const btVector3& inertiaInvA,
+					const btVector3& inertiaInvB)
+		: m_linearJointAxis(btVector3(btScalar(0.), btScalar(0.), btScalar(0.)))
 	{
-		m_aJ= world2A*jointAxis;
-		m_bJ = world2B*-jointAxis;
-		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_aJ = world2A * jointAxis;
+		m_bJ = world2B * -jointAxis;
+		m_0MinvJt = inertiaInvA * m_aJ;
 		m_1MinvJt = inertiaInvB * m_bJ;
-		m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+		m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
 
 		btAssert(m_Adiag > btScalar(0.0));
 	}
 
 	//angular constraint between two different rigidbodies
 	btJacobianEntry(const btVector3& axisInA,
-		const btVector3& axisInB,
-		const btVector3& inertiaInvA,
-		const btVector3& inertiaInvB)
-		: m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
-		, m_aJ(axisInA)
-		, m_bJ(-axisInB)
+					const btVector3& axisInB,
+					const btVector3& inertiaInvA,
+					const btVector3& inertiaInvB)
+		: m_linearJointAxis(btVector3(btScalar(0.), btScalar(0.), btScalar(0.))), m_aJ(axisInA), m_bJ(-axisInB)
 	{
-		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_0MinvJt = inertiaInvA * m_aJ;
 		m_1MinvJt = inertiaInvB * m_bJ;
-		m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+		m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
 
 		btAssert(m_Adiag > btScalar(0.0));
 	}
@@ -88,25 +86,25 @@ public:
 	//constraint on one rigidbody
 	btJacobianEntry(
 		const btMatrix3x3& world2A,
-		const btVector3& rel_pos1,const btVector3& rel_pos2,
+		const btVector3& rel_pos1, const btVector3& rel_pos2,
 		const btVector3& jointAxis,
-		const btVector3& inertiaInvA, 
+		const btVector3& inertiaInvA,
 		const btScalar massInvA)
-		:m_linearJointAxis(jointAxis)
+		: m_linearJointAxis(jointAxis)
 	{
-		m_aJ= world2A*(rel_pos1.cross(jointAxis));
-		m_bJ = world2A*(rel_pos2.cross(-jointAxis));
-		m_0MinvJt	= inertiaInvA * m_aJ;
-		m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.));
+		m_aJ = world2A * (rel_pos1.cross(jointAxis));
+		m_bJ = world2A * (rel_pos2.cross(-jointAxis));
+		m_0MinvJt = inertiaInvA * m_aJ;
+		m_1MinvJt = btVector3(btScalar(0.), btScalar(0.), btScalar(0.));
 		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
 
 		btAssert(m_Adiag > btScalar(0.0));
 	}
 
-	btScalar	getDiagonal() const { return m_Adiag; }
+	btScalar getDiagonal() const { return m_Adiag; }
 
 	// for two constraints on the same rigidbody (for example vehicle friction)
-	btScalar	getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const
+	btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const
 	{
 		const btJacobianEntry& jacA = *this;
 		btScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
@@ -114,42 +112,39 @@ public:
 		return lin + ang;
 	}
 
-	
-
 	// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
-	btScalar	getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const
+	btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA, const btScalar massInvB) const
 	{
 		const btJacobianEntry& jacA = *this;
 		btVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
 		btVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
 		btVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
-		btVector3 lin0 = massInvA * lin ;
+		btVector3 lin0 = massInvA * lin;
 		btVector3 lin1 = massInvB * lin;
-		btVector3 sum = ang0+ang1+lin0+lin1;
-		return sum[0]+sum[1]+sum[2];
+		btVector3 sum = ang0 + ang1 + lin0 + lin1;
+		return sum[0] + sum[1] + sum[2];
 	}
 
-	btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB)
+	btScalar getRelativeVelocity(const btVector3& linvelA, const btVector3& angvelA, const btVector3& linvelB, const btVector3& angvelB)
 	{
 		btVector3 linrel = linvelA - linvelB;
-		btVector3 angvela  = angvelA * m_aJ;
-		btVector3 angvelb  = angvelB * m_bJ;
+		btVector3 angvela = angvelA * m_aJ;
+		btVector3 angvelb = angvelB * m_bJ;
 		linrel *= m_linearJointAxis;
 		angvela += angvelb;
 		angvela += linrel;
-		btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
+		btScalar rel_vel2 = angvela[0] + angvela[1] + angvela[2];
 		return rel_vel2 + SIMD_EPSILON;
 	}
-//private:
+	//private:
 
-	btVector3	m_linearJointAxis;
-	btVector3	m_aJ;
-	btVector3	m_bJ;
-	btVector3	m_0MinvJt;
-	btVector3	m_1MinvJt;
+	btVector3 m_linearJointAxis;
+	btVector3 m_aJ;
+	btVector3 m_bJ;
+	btVector3 m_0MinvJt;
+	btVector3 m_1MinvJt;
 	//Optimization: can be stored in the w/last component of one of the vectors
-	btScalar	m_Adiag;
-
+	btScalar m_Adiag;
 };
 
-#endif //BT_JACOBIAN_ENTRY_H
+#endif  //BT_JACOBIAN_ENTRY_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp
index f110cd48081..ccf89160491 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp
@@ -15,14 +15,9 @@ subject to the following restrictions:
 
 #include "btNNCGConstraintSolver.h"
 
-
-
-
-
-
-btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
-	btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
+	btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
 	m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
 	m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
@@ -37,38 +32,39 @@ btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject*
 	return val;
 }
 
-btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
+btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/)
 {
-
 	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
 	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
 	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
 
 	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
 	{
-		if (1)			// uncomment this for a bit less random ((iteration & 7) == 0)
+		if (1)  // uncomment this for a bit less random ((iteration & 7) == 0)
 		{
-
-			for (int j=0; j<numNonContactPool; ++j) {
+			for (int j = 0; j < numNonContactPool; ++j)
+			{
 				int tmp = m_orderNonContactConstraintPool[j];
-				int swapi = btRandInt2(j+1);
+				int swapi = btRandInt2(j + 1);
 				m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
 				m_orderNonContactConstraintPool[swapi] = tmp;
 			}
 
-			//contact/friction constraints are not solved more than 
-			if (iteration< infoGlobal.m_numIterations)
+			//contact/friction constraints are not solved more than
+			if (iteration < infoGlobal.m_numIterations)
 			{
-				for (int j=0; j<numConstraintPool; ++j) {
+				for (int j = 0; j < numConstraintPool; ++j)
+				{
 					int tmp = m_orderTmpConstraintPool[j];
-					int swapi = btRandInt2(j+1);
+					int swapi = btRandInt2(j + 1);
 					m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
 					m_orderTmpConstraintPool[swapi] = tmp;
 				}
 
-				for (int j=0; j<numFrictionPool; ++j) {
+				for (int j = 0; j < numFrictionPool; ++j)
+				{
 					int tmp = m_orderFrictionConstraintPool[j];
-					int swapi = btRandInt2(j+1);
+					int swapi = btRandInt2(j + 1);
 					m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
 					m_orderFrictionConstraintPool[swapi] = tmp;
 				}
@@ -76,53 +72,40 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 		}
 	}
 
-
 	btScalar deltaflengthsqr = 0;
-
-	if (infoGlobal.m_solverMode & SOLVER_SIMD)
 	{
-		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
 		{
 			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-			if (iteration < constraint.m_overrideNumSolverIterations) 
+			if (iteration < constraint.m_overrideNumSolverIterations)
 			{
-				btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
-				m_deltafNC[j] = deltaf;
-				deltaflengthsqr += deltaf * deltaf;
-			}
-		}
-	} else 
-	{
-		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
-		{
-			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-			if (iteration < constraint.m_overrideNumSolverIterations) 
-			{
-				btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint);
 				m_deltafNC[j] = deltaf;
 				deltaflengthsqr += deltaf * deltaf;
 			}
 		}
 	}
 
-
-	if (m_onlyForNoneContact) 
+	if (m_onlyForNoneContact)
 	{
-		if (iteration==0) 
+		if (iteration == 0)
+		{
+			for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = m_deltafNC[j];
+		}
+		else
 		{
-			for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
-		} else {
 			// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
-			btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
-			if (beta>1) 
+			btScalar beta = m_deltafLengthSqrPrev > 0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
+			if (beta > 1)
 			{
-				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
-			} else 
+				for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = 0;
+			}
+			else
 			{
-				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+				for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
 				{
 					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-					if (iteration < constraint.m_overrideNumSolverIterations) 
+					if (iteration < constraint.m_overrideNumSolverIterations)
 					{
 						btScalar additionaldeltaimpulse = beta * m_pNC[j];
 						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
@@ -130,8 +113,8 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
 						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
 						const btSolverConstraint& c = constraint;
-						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
-						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+						body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
 					}
 				}
 			}
@@ -139,300 +122,225 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 		m_deltafLengthSqrPrev = deltaflengthsqr;
 	}
 
-
-
-	if (infoGlobal.m_solverMode & SOLVER_SIMD)
 	{
-
-		if (iteration< infoGlobal.m_numIterations)
+		if (iteration < infoGlobal.m_numIterations)
 		{
-			for (int j=0;j<numConstraints;j++)
+			for (int j = 0; j < numConstraints; j++)
 			{
 				if (constraints[j]->isEnabled())
 				{
-					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
-					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
+					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(), infoGlobal.m_timeStep);
+					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(), infoGlobal.m_timeStep);
 					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
 					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
-					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
+					constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
 				}
 			}
 
-			///solve all contact constraints using SIMD, if available
+			///solve all contact constraints
 			if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
 			{
 				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-				int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
+				int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
 
-				for (int c=0;c<numPoolConstraints;c++)
+				for (int c = 0; c < numPoolConstraints; c++)
 				{
-					btScalar totalImpulse =0;
+					btScalar totalImpulse = 0;
 
 					{
 						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
-						btScalar deltaf = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);						
+						btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 						m_deltafC[c] = deltaf;
-						deltaflengthsqr += deltaf*deltaf;
+						deltaflengthsqr += deltaf * deltaf;
 						totalImpulse = solveManifold.m_appliedImpulse;
 					}
 					bool applyFriction = true;
 					if (applyFriction)
 					{
 						{
+							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]];
 
-							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
-
-							if (totalImpulse>btScalar(0))
+							if (totalImpulse > btScalar(0))
 							{
-								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-								btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-								m_deltafCF[c*multiplier] = deltaf;
-								deltaflengthsqr += deltaf*deltaf;
-							} else {
-								m_deltafCF[c*multiplier] = 0;
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
+								btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+								m_deltafCF[c * multiplier] = deltaf;
+								deltaflengthsqr += deltaf * deltaf;
+							}
+							else
+							{
+								m_deltafCF[c * multiplier] = 0;
 							}
 						}
 
 						if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
 						{
+							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]];
 
-							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
-
-							if (totalImpulse>btScalar(0))
+							if (totalImpulse > btScalar(0))
+							{
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
+								btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+								m_deltafCF[c * multiplier + 1] = deltaf;
+								deltaflengthsqr += deltaf * deltaf;
+							}
+							else
 							{
-								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-								btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-								m_deltafCF[c*multiplier+1] = deltaf;
-								deltaflengthsqr += deltaf*deltaf;
-							} else {
-								m_deltafCF[c*multiplier+1] = 0;
+								m_deltafCF[c * multiplier + 1] = 0;
 							}
 						}
 					}
 				}
-
 			}
-			else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
+			else  //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
 			{
 				//solve the friction constraints after all contact constraints, don't interleave them
 				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
 				int j;
 
-				for (j=0;j<numPoolConstraints;j++)
+				for (j = 0; j < numPoolConstraints; j++)
 				{
 					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-					//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-					btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 					m_deltafC[j] = deltaf;
-					deltaflengthsqr += deltaf*deltaf;
+					deltaflengthsqr += deltaf * deltaf;
 				}
 
-
-
-				///solve all friction constraints, using SIMD, if available
+				///solve all friction constraints
 
 				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-				for (j=0;j<numFrictionPoolConstraints;j++)
+				for (j = 0; j < numFrictionPoolConstraints; j++)
 				{
 					btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
 					btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
 
-					if (totalImpulse>btScalar(0))
+					if (totalImpulse > btScalar(0))
 					{
-						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+						solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+						solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
 
-						//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-						btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 						m_deltafCF[j] = deltaf;
-						deltaflengthsqr += deltaf*deltaf;
-					} else {
+						deltaflengthsqr += deltaf * deltaf;
+					}
+					else
+					{
 						m_deltafCF[j] = 0;
 					}
 				}
+			}
 
-
+			{
 				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-				for (j=0;j<numRollingFrictionPoolConstraints;j++)
+				for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
 				{
-
 					btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
 					btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
-					if (totalImpulse>btScalar(0))
+					if (totalImpulse > btScalar(0))
 					{
-						btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
-						if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
+						btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
+						if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
 							rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
 
 						rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
 						rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
 
-						btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+						btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
 						m_deltafCRF[j] = deltaf;
-						deltaflengthsqr += deltaf*deltaf;
-					} else {
+						deltaflengthsqr += deltaf * deltaf;
+					}
+					else
+					{
 						m_deltafCRF[j] = 0;
 					}
 				}
-
-
 			}
 		}
+	}
 
-
-
-	} else
+	if (!m_onlyForNoneContact)
 	{
-
-		if (iteration< infoGlobal.m_numIterations)
+		if (iteration == 0)
 		{
-			for (int j=0;j<numConstraints;j++)
-			{
-				if (constraints[j]->isEnabled())
-				{
-					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
-					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
-					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
-					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
-					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
-				}
-			}
-			///solve all contact constraints
-			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-			for (int j=0;j<numPoolConstraints;j++)
-			{
-				const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-				btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-				m_deltafC[j] = deltaf;
-				deltaflengthsqr += deltaf*deltaf;
-			}
-			///solve all friction constraints
-			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-			for (int j=0;j<numFrictionPoolConstraints;j++)
-			{
-				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-				btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-
-				if (totalImpulse>btScalar(0))
-				{
-					solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-					solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-
-					btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-					m_deltafCF[j] = deltaf;
-					deltaflengthsqr += deltaf*deltaf;
-				} else {
-					m_deltafCF[j] = 0;
-				}
-			}
-
-			int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-			for (int j=0;j<numRollingFrictionPoolConstraints;j++)
-			{
-				btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
-				btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
-				if (totalImpulse>btScalar(0))
-				{
-					btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
-					if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
-						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
-
-					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
-					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
-
-					btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
-					m_deltafCRF[j] = deltaf;
-					deltaflengthsqr += deltaf*deltaf;
-				} else {
-					m_deltafCRF[j] = 0;
-				}
-			}
+			for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = m_deltafNC[j];
+			for (int j = 0; j < m_tmpSolverContactConstraintPool.size(); j++) m_pC[j] = m_deltafC[j];
+			for (int j = 0; j < m_tmpSolverContactFrictionConstraintPool.size(); j++) m_pCF[j] = m_deltafCF[j];
+			for (int j = 0; j < m_tmpSolverContactRollingFrictionConstraintPool.size(); j++) m_pCRF[j] = m_deltafCRF[j];
 		}
-	}
-
-
-
-
-	if (!m_onlyForNoneContact) 
-	{
-		if (iteration==0) 
+		else
 		{
-			for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
-			for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j];
-			for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[j];
-			if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) 
+			// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
+			btScalar beta = m_deltafLengthSqrPrev > 0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
+			if (beta > 1)
 			{
-				for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
+				for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) m_pNC[j] = 0;
+				for (int j = 0; j < m_tmpSolverContactConstraintPool.size(); j++) m_pC[j] = 0;
+				for (int j = 0; j < m_tmpSolverContactFrictionConstraintPool.size(); j++) m_pCF[j] = 0;
+				for (int j = 0; j < m_tmpSolverContactRollingFrictionConstraintPool.size(); j++) m_pCRF[j] = 0;
 			}
-		} else 
-		{
-			// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
-			btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
-			if (beta>1) {
-				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
-				for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0;
-				for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = 0;
-				if ( (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
-					for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
-				}
-			} else {
-				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+			else
+			{
+				for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
 				{
 					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-					if (iteration < constraint.m_overrideNumSolverIterations) {
+					if (iteration < constraint.m_overrideNumSolverIterations)
+					{
 						btScalar additionaldeltaimpulse = beta * m_pNC[j];
 						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
 						m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
 						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
 						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
 						const btSolverConstraint& c = constraint;
-						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
-						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+						body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
 					}
 				}
-				for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++)
+				for (int j = 0; j < m_tmpSolverContactConstraintPool.size(); j++)
 				{
 					btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-					if (iteration< infoGlobal.m_numIterations) {
+					if (iteration < infoGlobal.m_numIterations)
+					{
 						btScalar additionaldeltaimpulse = beta * m_pC[j];
 						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
 						m_pC[j] = beta * m_pC[j] + m_deltafC[j];
 						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
 						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
 						const btSolverConstraint& c = constraint;
-						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
-						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+						body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
 					}
 				}
-				for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++)
+				for (int j = 0; j < m_tmpSolverContactFrictionConstraintPool.size(); j++)
 				{
 					btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-					if (iteration< infoGlobal.m_numIterations) {
+					if (iteration < infoGlobal.m_numIterations)
+					{
 						btScalar additionaldeltaimpulse = beta * m_pCF[j];
 						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
 						m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j];
 						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
 						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
 						const btSolverConstraint& c = constraint;
-						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
-						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+						body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
 					}
 				}
-				if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
-					for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++)
+				{
+					for (int j = 0; j < m_tmpSolverContactRollingFrictionConstraintPool.size(); j++)
 					{
 						btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
-						if (iteration< infoGlobal.m_numIterations) {
+						if (iteration < infoGlobal.m_numIterations)
+						{
 							btScalar additionaldeltaimpulse = beta * m_pCRF[j];
 							constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
 							m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j];
 							btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
 							btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
 							const btSolverConstraint& c = constraint;
-							body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
-							body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+							body1.internalApplyImpulse(c.m_contactNormal1 * body1.internalGetInvMass(), c.m_angularComponentA, additionaldeltaimpulse);
+							body2.internalApplyImpulse(c.m_contactNormal2 * body2.internalGetInvMass(), c.m_angularComponentB, additionaldeltaimpulse);
 						}
 					}
 				}
@@ -444,7 +352,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
 	return deltaflengthsqr;
 }
 
-btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
+btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
 {
 	m_pNC.resizeNoInitialize(0);
 	m_pC.resizeNoInitialize(0);
@@ -458,6 +366,3 @@ btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject
 
 	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
 }
-
-
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h
index a300929cd5c..c84f274a991 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h
@@ -18,33 +18,30 @@ subject to the following restrictions:
 
 #include "btSequentialImpulseConstraintSolver.h"
 
-ATTRIBUTE_ALIGNED16(class) btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver
+ATTRIBUTE_ALIGNED16(class)
+btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver
 {
 protected:
-
 	btScalar m_deltafLengthSqrPrev;
 
-	btAlignedObjectArray<btScalar> m_pNC;  // p for None Contact constraints
-	btAlignedObjectArray<btScalar> m_pC;   // p for Contact constraints
-	btAlignedObjectArray<btScalar> m_pCF;  // p for ContactFriction constraints
-	btAlignedObjectArray<btScalar> m_pCRF; // p for ContactRollingFriction constraints
+	btAlignedObjectArray<btScalar> m_pNC;   // p for None Contact constraints
+	btAlignedObjectArray<btScalar> m_pC;    // p for Contact constraints
+	btAlignedObjectArray<btScalar> m_pCF;   // p for ContactFriction constraints
+	btAlignedObjectArray<btScalar> m_pCRF;  // p for ContactRollingFriction constraints
 
 	//These are recalculated in every iterations. We just keep these to prevent reallocation in each iteration.
-	btAlignedObjectArray<btScalar> m_deltafNC;  // deltaf for NoneContact constraints
-	btAlignedObjectArray<btScalar> m_deltafC;   // deltaf for Contact constraints
-	btAlignedObjectArray<btScalar> m_deltafCF;  // deltaf for ContactFriction constraints
-	btAlignedObjectArray<btScalar> m_deltafCRF; // deltaf for ContactRollingFriction constraints
+	btAlignedObjectArray<btScalar> m_deltafNC;   // deltaf for NoneContact constraints
+	btAlignedObjectArray<btScalar> m_deltafC;    // deltaf for Contact constraints
+	btAlignedObjectArray<btScalar> m_deltafCF;   // deltaf for ContactFriction constraints
+	btAlignedObjectArray<btScalar> m_deltafCRF;  // deltaf for ContactRollingFriction constraints
 
-		
 protected:
+	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
+	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 
-	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
-	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-
-	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {}
@@ -57,8 +54,4 @@ public:
 	bool m_onlyForNoneContact;
 };
 
-
-
-
-#endif //BT_NNCG_CONSTRAINT_SOLVER_H
-
+#endif  //BT_NNCG_CONSTRAINT_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
index 3c0430b903f..ad399dc57f7 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
@@ -13,217 +13,193 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btPoint2PointConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include <new>
 
-
-
-
-
-btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
-:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
-m_flags(0),
-m_useSolveConstraintObsolete(false)
+btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& pivotInA, const btVector3& pivotInB)
+	: btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0), m_useSolveConstraintObsolete(false)
 {
-
 }
 
-
-btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
-:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
-m_flags(0),
-m_useSolveConstraintObsolete(false)
+btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA, const btVector3& pivotInA)
+	: btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE, rbA), m_pivotInA(pivotInA), m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), m_flags(0), m_useSolveConstraintObsolete(false)
 {
-	
 }
 
-void	btPoint2PointConstraint::buildJacobian()
+void btPoint2PointConstraint::buildJacobian()
 {
-
 	///we need it for both methods
 	{
 		m_appliedImpulse = btScalar(0.);
 
-		btVector3	normal(0,0,0);
+		btVector3 normal(0, 0, 0);
 
-		for (int i=0;i<3;i++)
+		for (int i = 0; i < 3; i++)
 		{
 			normal[i] = 1;
 			new (&m_jac[i]) btJacobianEntry(
-			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-			m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
-			m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
-			normal,
-			m_rbA.getInvInertiaDiagLocal(),
-			m_rbA.getInvMass(),
-			m_rbB.getInvInertiaDiagLocal(),
-			m_rbB.getInvMass());
-		normal[i] = 0;
+				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+				m_rbA.getCenterOfMassTransform() * m_pivotInA - m_rbA.getCenterOfMassPosition(),
+				m_rbB.getCenterOfMassTransform() * m_pivotInB - m_rbB.getCenterOfMassPosition(),
+				normal,
+				m_rbA.getInvInertiaDiagLocal(),
+				m_rbA.getInvMass(),
+				m_rbB.getInvInertiaDiagLocal(),
+				m_rbB.getInvMass());
+			normal[i] = 0;
 		}
 	}
-
-
 }
 
-void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
+void btPoint2PointConstraint::getInfo1(btConstraintInfo1* info)
 {
 	getInfo1NonVirtual(info);
 }
 
-void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+void btPoint2PointConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
 {
 	if (m_useSolveConstraintObsolete)
 	{
 		info->m_numConstraintRows = 0;
 		info->nub = 0;
-	} else
+	}
+	else
 	{
 		info->m_numConstraintRows = 3;
 		info->nub = 3;
 	}
 }
 
-
-
-
-void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
+void btPoint2PointConstraint::getInfo2(btConstraintInfo2* info)
 {
-	getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 }
 
-void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
+void btPoint2PointConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
 {
 	btAssert(!m_useSolveConstraintObsolete);
 
-	 //retrieve matrices
+	//retrieve matrices
 
 	// anchor points in global coordinates with respect to body PORs.
-   
-    // set jacobian
-    info->m_J1linearAxis[0] = 1;
-	info->m_J1linearAxis[info->rowskip+1] = 1;
-	info->m_J1linearAxis[2*info->rowskip+2] = 1;
 
-	btVector3 a1 = body0_trans.getBasis()*getPivotInA();
+	// set jacobian
+	info->m_J1linearAxis[0] = 1;
+	info->m_J1linearAxis[info->rowskip + 1] = 1;
+	info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
+
+	btVector3 a1 = body0_trans.getBasis() * getPivotInA();
 	{
 		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
-		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * info->rowskip);
 		btVector3 a1neg = -a1;
-		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
-    
+
 	info->m_J2linearAxis[0] = -1;
-    info->m_J2linearAxis[info->rowskip+1] = -1;
-    info->m_J2linearAxis[2*info->rowskip+2] = -1;
-	
-	btVector3 a2 = body1_trans.getBasis()*getPivotInB();
-   
+	info->m_J2linearAxis[info->rowskip + 1] = -1;
+	info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
+
+	btVector3 a2 = body1_trans.getBasis() * getPivotInB();
+
 	{
-	//	btVector3 a2n = -a2;
+		//	btVector3 a2n = -a2;
 		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
-		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
-		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
-    
-
 
-    // set right hand side
+	// set right hand side
 	btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
-    btScalar k = info->fps * currERP;
-    int j;
-	for (j=0; j<3; j++)
-    {
-        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
+	btScalar k = info->fps * currERP;
+	int j;
+	for (j = 0; j < 3; j++)
+	{
+		info->m_constraintError[j * info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
 		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
-    }
-	if(m_flags & BT_P2P_FLAGS_CFM)
+	}
+	if (m_flags & BT_P2P_FLAGS_CFM)
 	{
-		for (j=0; j<3; j++)
+		for (j = 0; j < 3; j++)
 		{
-			info->cfm[j*info->rowskip] = m_cfm;
+			info->cfm[j * info->rowskip] = m_cfm;
 		}
 	}
 
-	btScalar impulseClamp = m_setting.m_impulseClamp;//
-	for (j=0; j<3; j++)
-    {
+	btScalar impulseClamp = m_setting.m_impulseClamp;  //
+	for (j = 0; j < 3; j++)
+	{
 		if (m_setting.m_impulseClamp > 0)
 		{
-			info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
-			info->m_upperLimit[j*info->rowskip] = impulseClamp;
+			info->m_lowerLimit[j * info->rowskip] = -impulseClamp;
+			info->m_upperLimit[j * info->rowskip] = impulseClamp;
 		}
 	}
 	info->m_damping = m_setting.m_damping;
-	
 }
 
-
-
-void	btPoint2PointConstraint::updateRHS(btScalar	timeStep)
+void btPoint2PointConstraint::updateRHS(btScalar timeStep)
 {
 	(void)timeStep;
-
 }
 
-///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 ///If no axis is provided, it uses the default axis for this constraint.
 void btPoint2PointConstraint::setParam(int num, btScalar value, int axis)
 {
-	if(axis != -1)
+	if (axis != -1)
 	{
 		btAssertConstrParams(0);
 	}
 	else
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_ERP :
-			case BT_CONSTRAINT_STOP_ERP :
-				m_erp = value; 
+			case BT_CONSTRAINT_ERP:
+			case BT_CONSTRAINT_STOP_ERP:
+				m_erp = value;
 				m_flags |= BT_P2P_FLAGS_ERP;
 				break;
-			case BT_CONSTRAINT_CFM :
-			case BT_CONSTRAINT_STOP_CFM :
-				m_cfm = value; 
+			case BT_CONSTRAINT_CFM:
+			case BT_CONSTRAINT_STOP_CFM:
+				m_cfm = value;
 				m_flags |= BT_P2P_FLAGS_CFM;
 				break;
-			default: 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
 }
 
 ///return the local value of parameter
-btScalar btPoint2PointConstraint::getParam(int num, int axis) const 
+btScalar btPoint2PointConstraint::getParam(int num, int axis) const
 {
 	btScalar retVal(SIMD_INFINITY);
-	if(axis != -1)
+	if (axis != -1)
 	{
 		btAssertConstrParams(0);
 	}
 	else
 	{
-		switch(num)
+		switch (num)
 		{
-			case BT_CONSTRAINT_ERP :
-			case BT_CONSTRAINT_STOP_ERP :
+			case BT_CONSTRAINT_ERP:
+			case BT_CONSTRAINT_STOP_ERP:
 				btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
-				retVal = m_erp; 
+				retVal = m_erp;
 				break;
-			case BT_CONSTRAINT_CFM :
-			case BT_CONSTRAINT_STOP_CFM :
+			case BT_CONSTRAINT_CFM:
+			case BT_CONSTRAINT_STOP_CFM:
 				btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
-				retVal = m_cfm; 
+				retVal = m_cfm;
 				break;
-			default: 
+			default:
 				btAssertConstrParams(0);
 		}
 	}
 	return retVal;
 }
-	
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
index 8fa03d719d7..4717e198004 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
@@ -22,26 +22,24 @@ subject to the following restrictions:
 
 class btRigidBody;
 
-
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btPoint2PointConstraintData2	btPoint2PointConstraintDoubleData2
-#define btPoint2PointConstraintDataName	"btPoint2PointConstraintDoubleData2"
+#define btPoint2PointConstraintData2 btPoint2PointConstraintDoubleData2
+#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData2"
 #else
-#define btPoint2PointConstraintData2	btPoint2PointConstraintFloatData
-#define btPoint2PointConstraintDataName	"btPoint2PointConstraintFloatData"
-#endif //BT_USE_DOUBLE_PRECISION
+#define btPoint2PointConstraintData2 btPoint2PointConstraintFloatData
+#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
-struct	btConstraintSetting
+struct btConstraintSetting
 {
-	btConstraintSetting()	:
-		m_tau(btScalar(0.3)),
-		m_damping(btScalar(1.)),
-		m_impulseClamp(btScalar(0.))
+	btConstraintSetting() : m_tau(btScalar(0.3)),
+							m_damping(btScalar(1.)),
+							m_impulseClamp(btScalar(0.))
 	{
 	}
-	btScalar		m_tau;
-	btScalar		m_damping;
-	btScalar		m_impulseClamp;
+	btScalar m_tau;
+	btScalar m_damping;
+	btScalar m_impulseClamp;
 };
 
 enum btPoint2PointFlags
@@ -51,52 +49,51 @@ enum btPoint2PointFlags
 };
 
 /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
-ATTRIBUTE_ALIGNED16(class) btPoint2PointConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class)
+btPoint2PointConstraint : public btTypedConstraint
 {
 #ifdef IN_PARALLELL_SOLVER
 public:
 #endif
-	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
-	
-	btVector3	m_pivotInA;
-	btVector3	m_pivotInB;
-	
-	int			m_flags;
-	btScalar	m_erp;
-	btScalar	m_cfm;
-	
-public:
+	btJacobianEntry m_jac[3];  //3 orthogonal linear constraints
+
+	btVector3 m_pivotInA;
+	btVector3 m_pivotInB;
+
+	int m_flags;
+	btScalar m_erp;
+	btScalar m_cfm;
 
+public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	///for backwards compatibility during the transition to 'getInfo/getInfo2'
-	bool		m_useSolveConstraintObsolete;
-
-	btConstraintSetting	m_setting;
+	bool m_useSolveConstraintObsolete;
 
-	btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB);
+	btConstraintSetting m_setting;
 
-	btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);
+	btPoint2PointConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& pivotInA, const btVector3& pivotInB);
 
+	btPoint2PointConstraint(btRigidBody & rbA, const btVector3& pivotInA);
 
-	virtual void	buildJacobian();
+	virtual void buildJacobian();
 
-	virtual void getInfo1 (btConstraintInfo1* info);
+	virtual void getInfo1(btConstraintInfo1 * info);
 
-	void getInfo1NonVirtual (btConstraintInfo1* info);
+	void getInfo1NonVirtual(btConstraintInfo1 * info);
 
-	virtual void getInfo2 (btConstraintInfo2* info);
+	virtual void getInfo2(btConstraintInfo2 * info);
 
-	void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans);
+	void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& body0_trans, const btTransform& body1_trans);
 
-	void	updateRHS(btScalar	timeStep);
+	void updateRHS(btScalar timeStep);
 
-	void	setPivotA(const btVector3& pivotA)
+	void setPivotA(const btVector3& pivotA)
 	{
 		m_pivotInA = pivotA;
 	}
 
-	void	setPivotB(const btVector3& pivotB)
+	void setPivotB(const btVector3& pivotB)
 	{
 		m_pivotInB = pivotB;
 	}
@@ -111,70 +108,66 @@ public:
 		return m_pivotInB;
 	}
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 	///If no axis is provided, it uses the default axis for this constraint.
-	virtual	void	setParam(int num, btScalar value, int axis = -1);
+	virtual void setParam(int num, btScalar value, int axis = -1);
 	///return the local value of parameter
-	virtual	btScalar getParam(int num, int axis = -1) const;
-	
-	virtual	int getFlags() const
+	virtual btScalar getParam(int num, int axis = -1) const;
+
+	virtual int getFlags() const
 	{
-        	return m_flags;
-    	}
+		return m_flags;
+	}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	virtual int calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
-
-
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btPoint2PointConstraintFloatData
+struct btPoint2PointConstraintFloatData
 {
-	btTypedConstraintData	m_typeConstraintData;
-	btVector3FloatData	m_pivotInA;
-	btVector3FloatData	m_pivotInB;
+	btTypedConstraintData m_typeConstraintData;
+	btVector3FloatData m_pivotInA;
+	btVector3FloatData m_pivotInB;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btPoint2PointConstraintDoubleData2
+struct btPoint2PointConstraintDoubleData2
 {
-	btTypedConstraintDoubleData	m_typeConstraintData;
-	btVector3DoubleData	m_pivotInA;
-	btVector3DoubleData	m_pivotInB;
+	btTypedConstraintDoubleData m_typeConstraintData;
+	btVector3DoubleData m_pivotInA;
+	btVector3DoubleData m_pivotInB;
 };
 
 #ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 ///this structure is not used, except for loading pre-2.82 .bullet files
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btPoint2PointConstraintDoubleData
+struct btPoint2PointConstraintDoubleData
 {
-	btTypedConstraintData	m_typeConstraintData;
-	btVector3DoubleData	m_pivotInA;
-	btVector3DoubleData	m_pivotInB;
+	btTypedConstraintData m_typeConstraintData;
+	btVector3DoubleData m_pivotInA;
+	btVector3DoubleData m_pivotInB;
 };
-#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
+#endif  //BT_BACKWARDS_COMPATIBLE_SERIALIZATION
 
-
-SIMD_FORCE_INLINE	int	btPoint2PointConstraint::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btPoint2PointConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(btPoint2PointConstraintData2);
-
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char* btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
 	btPoint2PointConstraintData2* p2pData = (btPoint2PointConstraintData2*)dataBuffer;
 
-	btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
+	btTypedConstraint::serialize(&p2pData->m_typeConstraintData, serializer);
 	m_pivotInA.serialize(p2pData->m_pivotInA);
 	m_pivotInB.serialize(p2pData->m_pivotInB);
 
 	return btPoint2PointConstraintDataName;
 }
 
-#endif //BT_POINT2POINTCONSTRAINT_H
+#endif  //BT_POINT2POINTCONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index 8da572bf7d8..d2641c582f1 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -14,7 +14,9 @@ subject to the following restrictions:
 */
 
 //#define COMPUTE_IMPULSE_DENOM 1
-//#define BT_ADDITIONAL_DEBUG
+#ifdef BT_DEBUG
+#	define BT_ADDITIONAL_DEBUG
+#endif
 
 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
 
@@ -33,24 +35,24 @@ subject to the following restrictions:
 //#include "btSolverBody.h"
 //#include "btSolverConstraint.h"
 #include "LinearMath/btAlignedObjectArray.h"
-#include <string.h> //for memset
+#include <string.h>  //for memset
 
-int		gNumSplitImpulseRecoveries = 0;
+int gNumSplitImpulseRecoveries = 0;
 
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 
-
+//#define VERBOSE_RESIDUAL_PRINTF 1
 ///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver
 ///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check.
-static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
+static btScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
-	btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
-	const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
-	const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
+	btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm;
+	const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity());
+	const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity());
 
 	//	const btScalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;
-	deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
-	deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
+	deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
+	deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
 
 	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
 	if (sum < c.m_lowerLimit)
@@ -68,21 +70,20 @@ static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolver
 		c.m_appliedImpulse = sum;
 	}
 
-	body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
-	body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+	bodyA.internalApplyImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+	bodyB.internalApplyImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
 
-	return deltaImpulse;
+	return deltaImpulse * (1. / c.m_jacDiagABInv);
 }
 
-
-static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
+static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
-	btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
-	const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
-	const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
+	btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm;
+	const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity());
+	const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity());
 
-	deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
-	deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
+	deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
+	deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
 	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
 	if (sum < c.m_lowerLimit)
 	{
@@ -93,70 +94,67 @@ static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSol
 	{
 		c.m_appliedImpulse = sum;
 	}
-	body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
-	body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+	bodyA.internalApplyImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+	bodyB.internalApplyImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
 
-	return deltaImpulse;
+	return deltaImpulse * (1. / c.m_jacDiagABInv);
 }
 
-
-
 #ifdef USE_SIMD
 #include <emmintrin.h>
 
-
-#define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
-static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
+#define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e, e, e, e))
+static inline __m128 btSimdDot3(__m128 vec0, __m128 vec1)
 {
-	__m128 result = _mm_mul_ps( vec0, vec1);
-	return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) );
+	__m128 result = _mm_mul_ps(vec0, vec1);
+	return _mm_add_ps(btVecSplat(result, 0), _mm_add_ps(btVecSplat(result, 1), btVecSplat(result, 2)));
 }
 
-#if defined (BT_ALLOW_SSE4)
+#if defined(BT_ALLOW_SSE4)
 #include <intrin.h>
 
-#define USE_FMA					1
-#define USE_FMA3_INSTEAD_FMA4	1
-#define USE_SSE4_DOT			1
+#define USE_FMA 1
+#define USE_FMA3_INSTEAD_FMA4 1
+#define USE_SSE4_DOT 1
 
-#define SSE4_DP(a, b)			_mm_dp_ps(a, b, 0x7f)
-#define SSE4_DP_FP(a, b)		_mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f))
+#define SSE4_DP(a, b) _mm_dp_ps(a, b, 0x7f)
+#define SSE4_DP_FP(a, b) _mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f))
 
 #if USE_SSE4_DOT
-#define DOT_PRODUCT(a, b)		SSE4_DP(a, b)
+#define DOT_PRODUCT(a, b) SSE4_DP(a, b)
 #else
-#define DOT_PRODUCT(a, b)		btSimdDot3(a, b)
+#define DOT_PRODUCT(a, b) btSimdDot3(a, b)
 #endif
 
 #if USE_FMA
 #if USE_FMA3_INSTEAD_FMA4
 // a*b + c
-#define FMADD(a, b, c)		_mm_fmadd_ps(a, b, c)
+#define FMADD(a, b, c) _mm_fmadd_ps(a, b, c)
 // -(a*b) + c
-#define FMNADD(a, b, c)		_mm_fnmadd_ps(a, b, c)
-#else // USE_FMA3
+#define FMNADD(a, b, c) _mm_fnmadd_ps(a, b, c)
+#else  // USE_FMA3
 // a*b + c
-#define FMADD(a, b, c)		_mm_macc_ps(a, b, c)
+#define FMADD(a, b, c) _mm_macc_ps(a, b, c)
 // -(a*b) + c
-#define FMNADD(a, b, c)		_mm_nmacc_ps(a, b, c)
+#define FMNADD(a, b, c) _mm_nmacc_ps(a, b, c)
 #endif
-#else // USE_FMA
+#else  // USE_FMA
 // c + a*b
-#define FMADD(a, b, c)		_mm_add_ps(c, _mm_mul_ps(a, b))
+#define FMADD(a, b, c) _mm_add_ps(c, _mm_mul_ps(a, b))
 // c - a*b
-#define FMNADD(a, b, c)		_mm_sub_ps(c, _mm_mul_ps(a, b))
+#define FMNADD(a, b, c) _mm_sub_ps(c, _mm_mul_ps(a, b))
 #endif
 #endif
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
-static btSimdScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
+static btScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
 	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
-	__m128	lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
-	__m128	upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+	__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+	__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
 	btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm)));
-	__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
-	__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
+	__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
+	__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
 	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
 	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
 	btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
@@ -169,54 +167,52 @@ static btSimdScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1,
 	__m128 upperMinApplied = _mm_sub_ps(upperLimit1, cpAppliedImp);
 	deltaImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied));
 	c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1));
-	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128);
-	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, body2.internalGetInvMass().mVec128);
+	__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
+	__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, bodyB.internalGetInvMass().mVec128);
 	__m128 impulseMagnitude = deltaImpulse;
-	body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
-	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
-	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
-	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
-	return deltaImpulse;
+	bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
+	bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
+	bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
+	bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
+	return deltaImpulse.m_floats[0] / c.m_jacDiagABInv;
 }
 
-
 // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
-static btSimdScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
+static btScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
-#if defined (BT_ALLOW_SSE4)
-	__m128 tmp					= _mm_set_ps1(c.m_jacDiagABInv);
-	__m128 deltaImpulse			= _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm);
-	const __m128 lowerLimit		= _mm_set_ps1(c.m_lowerLimit);
-	const __m128 upperLimit		= _mm_set_ps1(c.m_upperLimit);
-	const __m128 deltaVel1Dotn	= _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
-	const __m128 deltaVel2Dotn	= _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
-	deltaImpulse				= FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
-	deltaImpulse				= FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
-	tmp							= _mm_add_ps(c.m_appliedImpulse, deltaImpulse); // sum
-	const __m128 maskLower		= _mm_cmpgt_ps(tmp, lowerLimit);
-	const __m128 maskUpper		= _mm_cmpgt_ps(upperLimit, tmp);
-	deltaImpulse				= _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower);
-	c.m_appliedImpulse			= _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower);
-	body1.internalGetDeltaLinearVelocity().mVec128	= FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128);
-	body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128);
-	body2.internalGetDeltaLinearVelocity().mVec128	= FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128);
-	body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128);
-	return deltaImpulse;
+#if defined(BT_ALLOW_SSE4)
+	__m128 tmp = _mm_set_ps1(c.m_jacDiagABInv);
+	__m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm);
+	const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit);
+	const __m128 upperLimit = _mm_set_ps1(c.m_upperLimit);
+	const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
+	const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
+	deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
+	deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
+	tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse);  // sum
+	const __m128 maskLower = _mm_cmpgt_ps(tmp, lowerLimit);
+	const __m128 maskUpper = _mm_cmpgt_ps(upperLimit, tmp);
+	deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower);
+	c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower);
+	bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128);
+	bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128);
+	bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128);
+	bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128);
+	btSimdScalar deltaImp = deltaImpulse;
+	return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv);
 #else
-	return gResolveSingleConstraintRowGeneric_sse2(body1,body2,c);
+	return gResolveSingleConstraintRowGeneric_sse2(bodyA, bodyB, c);
 #endif
 }
 
-
-
-static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
+static btScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
 	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
-	__m128	lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
-	__m128	upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+	__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+	__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
 	btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm)));
-	__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
-	__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
+	__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
+	__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
 	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
 	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
 	btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
@@ -226,211 +222,209 @@ static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& bod
 	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
 	deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
 	c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
-	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128);
-	__m128	linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128);
+	__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
+	__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128);
 	__m128 impulseMagnitude = deltaImpulse;
-	body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
-	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
-	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
-	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
-	return deltaImpulse;
+	bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
+	bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
+	bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
+	bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
+	return deltaImpulse.m_floats[0] / c.m_jacDiagABInv;
 }
 
-
 // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
-static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
+static btScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
 #ifdef BT_ALLOW_SSE4
-	__m128 tmp					= _mm_set_ps1(c.m_jacDiagABInv);
-	__m128 deltaImpulse			= _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm);
-	const __m128 lowerLimit		= _mm_set_ps1(c.m_lowerLimit);
-	const __m128 deltaVel1Dotn	= _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
-	const __m128 deltaVel2Dotn	= _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
-	deltaImpulse				= FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
-	deltaImpulse				= FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
-	tmp							= _mm_add_ps(c.m_appliedImpulse, deltaImpulse);
-	const __m128 mask			= _mm_cmpgt_ps(tmp, lowerLimit);
-	deltaImpulse				= _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask);
-	c.m_appliedImpulse			= _mm_blendv_ps(lowerLimit, tmp, mask);
-	body1.internalGetDeltaLinearVelocity().mVec128	= FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128);
-	body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128);
-	body2.internalGetDeltaLinearVelocity().mVec128	= FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128);
-	body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128);
-	return deltaImpulse;
+	__m128 tmp = _mm_set_ps1(c.m_jacDiagABInv);
+	__m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm);
+	const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit);
+	const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
+	const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
+	deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
+	deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
+	tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse);
+	const __m128 mask = _mm_cmpgt_ps(tmp, lowerLimit);
+	deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask);
+	c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, tmp, mask);
+	bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128);
+	bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128);
+	bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128);
+	bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128);
+	btSimdScalar deltaImp = deltaImpulse;
+	return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv);
 #else
-	return gResolveSingleConstraintRowLowerLimit_sse2(body1,body2,c);
-#endif //BT_ALLOW_SSE4
+	return gResolveSingleConstraintRowLowerLimit_sse2(bodyA, bodyB, c);
+#endif  //BT_ALLOW_SSE4
 }
 
+#endif  //USE_SIMD
 
-#endif //USE_SIMD
-
-
-
-btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
-#ifdef USE_SIMD
-	return m_resolveSingleConstraintRowGeneric(body1, body2, c);
-#else
-	return resolveSingleConstraintRowGeneric(body1,body2,c);
-#endif
+	return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c);
 }
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
-btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
-	return gResolveSingleConstraintRowGeneric_scalar_reference(body1, body2, c);
+	return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c);
 }
 
-btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
-#ifdef USE_SIMD
-	return m_resolveSingleConstraintRowLowerLimit(body1, body2, c);
-#else
-	return resolveSingleConstraintRowLowerLimit(body1,body2,c);
-#endif
+	return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c);
 }
 
-
-btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
-	return gResolveSingleConstraintRowLowerLimit_scalar_reference(body1,body2,c);
+	return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c);
 }
 
-
-void	btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
-        btSolverBody& body1,
-        btSolverBody& body2,
-        const btSolverConstraint& c)
+static btScalar gResolveSplitPenetrationImpulse_scalar_reference(
+	btSolverBody& bodyA,
+	btSolverBody& bodyB,
+	const btSolverConstraint& c)
 {
-		if (c.m_rhsPenetration)
-        {
-			gNumSplitImpulseRecoveries++;
-			btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
-			const btScalar deltaVel1Dotn	=	c.m_contactNormal1.dot(body1.internalGetPushVelocity()) 	+ c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
-			const btScalar deltaVel2Dotn	=	c.m_contactNormal2.dot(body2.internalGetPushVelocity())		+ c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
-
-			deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
-			deltaImpulse	-=	deltaVel2Dotn*c.m_jacDiagABInv;
-			const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
-			if (sum < c.m_lowerLimit)
-			{
-				deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
-				c.m_appliedPushImpulse = c.m_lowerLimit;
-			}
-			else
-			{
-				c.m_appliedPushImpulse = sum;
-			}
-			body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
-			body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
-        }
+	btScalar deltaImpulse = 0.f;
+
+	if (c.m_rhsPenetration)
+	{
+		gNumSplitImpulseRecoveries++;
+		deltaImpulse = c.m_rhsPenetration - btScalar(c.m_appliedPushImpulse) * c.m_cfm;
+		const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetTurnVelocity());
+		const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetTurnVelocity());
+
+		deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
+		deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
+		const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
+		if (sum < c.m_lowerLimit)
+		{
+			deltaImpulse = c.m_lowerLimit - c.m_appliedPushImpulse;
+			c.m_appliedPushImpulse = c.m_lowerLimit;
+		}
+		else
+		{
+			c.m_appliedPushImpulse = sum;
+		}
+		bodyA.internalApplyPushImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+		bodyB.internalApplyPushImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+	}
+	return deltaImpulse * (1. / c.m_jacDiagABInv);
 }
 
- void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
 {
 #ifdef USE_SIMD
 	if (!c.m_rhsPenetration)
-		return;
+		return 0.f;
 
 	gNumSplitImpulseRecoveries++;
 
 	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
-	__m128	lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
-	__m128	upperLimit1 = _mm_set1_ps(c.m_upperLimit);
-	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm)));
-	__m128 deltaVel1Dotn	=	_mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
-	__m128 deltaVel2Dotn	=	_mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128));
-	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
-	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
-	btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
-	btSimdScalar resultLowerLess,resultUpperLess;
-	resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
-	resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
-	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
-	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
-	c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
-	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
-	__m128	linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128);
+	__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+	__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse), _mm_set1_ps(c.m_cfm)));
+	__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetTurnVelocity().mVec128));
+	__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetTurnVelocity().mVec128));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
+	btSimdScalar resultLowerLess, resultUpperLess;
+	resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
+	resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
+	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
+	deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
+	c.m_appliedPushImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
+	__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
+	__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128);
 	__m128 impulseMagnitude = deltaImpulse;
-	body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
-	body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
-	body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
-	body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+	bodyA.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyA.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
+	bodyA.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyA.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
+	bodyB.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyB.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
+	bodyB.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyB.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
+	btSimdScalar deltaImp = deltaImpulse;
+	return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv);
 #else
-	resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
+	return gResolveSplitPenetrationImpulse_scalar_reference(bodyA, bodyB, c);
 #endif
 }
 
+btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
+{
+	m_btSeed2 = 0;
+	m_cachedSolverMode = 0;
+	setupSolverFunctions(false);
+}
 
- btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
-	 : m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference),
-	 m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference),
-	 m_btSeed2(0)
- {
+void btSequentialImpulseConstraintSolver::setupSolverFunctions(bool useSimd)
+{
+	m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_scalar_reference;
+	m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_scalar_reference;
+	m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_scalar_reference;
 
+	if (useSimd)
+	{
 #ifdef USE_SIMD
-	 m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
-	 m_resolveSingleConstraintRowLowerLimit=gResolveSingleConstraintRowLowerLimit_sse2;
-#endif //USE_SIMD
+		m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
+		m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse2;
+		m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_sse2;
 
 #ifdef BT_ALLOW_SSE4
-	 int cpuFeatures = btCpuFeatureUtility::getCpuFeatures();
-	 if ((cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_FMA3) && (cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_SSE4_1))
-	 {
-		m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3;
-		m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
-	 }
-#endif//BT_ALLOW_SSE4
-
- }
-
- btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
- {
- }
+		int cpuFeatures = btCpuFeatureUtility::getCpuFeatures();
+		if ((cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_FMA3) && (cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_SSE4_1))
+		{
+			m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3;
+			m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
+		}
+#endif  //BT_ALLOW_SSE4
+#endif  //USE_SIMD
+	}
+}
 
- btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric()
- {
-	 return gResolveSingleConstraintRowGeneric_scalar_reference;
- }
+btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
+{
+}
 
- btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit()
- {
-	 return gResolveSingleConstraintRowLowerLimit_scalar_reference;
- }
+btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric()
+{
+	return gResolveSingleConstraintRowGeneric_scalar_reference;
+}
 
+btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit()
+{
+	return gResolveSingleConstraintRowLowerLimit_scalar_reference;
+}
 
 #ifdef USE_SIMD
- btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric()
- {
-	 return gResolveSingleConstraintRowGeneric_sse2;
- }
- btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit()
- {
-	 return gResolveSingleConstraintRowLowerLimit_sse2;
- }
+btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric()
+{
+	return gResolveSingleConstraintRowGeneric_sse2;
+}
+btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit()
+{
+	return gResolveSingleConstraintRowLowerLimit_sse2;
+}
 #ifdef BT_ALLOW_SSE4
- btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric()
- {
-	 return gResolveSingleConstraintRowGeneric_sse4_1_fma3;
- }
- btSingleConstraintRowSolver	btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit()
- {
-	 return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
- }
-#endif //BT_ALLOW_SSE4
-#endif //USE_SIMD
+btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric()
+{
+	return gResolveSingleConstraintRowGeneric_sse4_1_fma3;
+}
+btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit()
+{
+	return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
+}
+#endif  //BT_ALLOW_SSE4
+#endif  //USE_SIMD
 
 unsigned long btSequentialImpulseConstraintSolver::btRand2()
 {
-	m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
+	m_btSeed2 = (1664525L * m_btSeed2 + 1013904223L) & 0xffffffff;
 	return m_btSeed2;
 }
 
-
-
 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
-int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
+int btSequentialImpulseConstraintSolver::btRandInt2(int n)
 {
 	// seems good; xor-fold and modulus
 	const unsigned long un = static_cast<unsigned long>(n);
@@ -438,15 +432,20 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
 
 	// note: probably more aggressive than it needs to be -- might be
 	//       able to get away without one or two of the innermost branches.
-	if (un <= 0x00010000UL) {
+	if (un <= 0x00010000UL)
+	{
 		r ^= (r >> 16);
-		if (un <= 0x00000100UL) {
+		if (un <= 0x00000100UL)
+		{
 			r ^= (r >> 8);
-			if (un <= 0x00000010UL) {
+			if (un <= 0x00000010UL)
+			{
 				r ^= (r >> 4);
-				if (un <= 0x00000004UL) {
+				if (un <= 0x00000004UL)
+				{
 					r ^= (r >> 2);
-					if (un <= 0x00000002UL) {
+					if (un <= 0x00000002UL)
+					{
 						r ^= (r >> 1);
 					}
 				}
@@ -454,66 +453,56 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
 		}
 	}
 
-	return (int) (r % un);
+	return (int)(r % un);
 }
 
-
-
-void	btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep)
+void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep)
 {
+	btRigidBody* rb = collisionObject ? btRigidBody::upcast(collisionObject) : 0;
 
-	btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
-
-	solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
-	solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
-	solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
-	solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
+	solverBody->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f);
+	solverBody->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f);
+	solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
+	solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
 
 	if (rb)
 	{
 		solverBody->m_worldTransform = rb->getWorldTransform();
-		solverBody->internalSetInvMass(btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor());
+		solverBody->internalSetInvMass(btVector3(rb->getInvMass(), rb->getInvMass(), rb->getInvMass()) * rb->getLinearFactor());
 		solverBody->m_originalBody = rb;
 		solverBody->m_angularFactor = rb->getAngularFactor();
 		solverBody->m_linearFactor = rb->getLinearFactor();
 		solverBody->m_linearVelocity = rb->getLinearVelocity();
 		solverBody->m_angularVelocity = rb->getAngularVelocity();
-		solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep;
-		solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ;
-
-	} else
-	{
+		solverBody->m_externalForceImpulse = rb->getTotalForce() * rb->getInvMass() * timeStep;
+		solverBody->m_externalTorqueImpulse = rb->getTotalTorque() * rb->getInvInertiaTensorWorld() * timeStep;
+	}
+	else
+		{
 		solverBody->m_worldTransform.setIdentity();
-		solverBody->internalSetInvMass(btVector3(0,0,0));
+		solverBody->internalSetInvMass(btVector3(0, 0, 0));
 		solverBody->m_originalBody = 0;
-		solverBody->m_angularFactor.setValue(1,1,1);
-		solverBody->m_linearFactor.setValue(1,1,1);
-		solverBody->m_linearVelocity.setValue(0,0,0);
-		solverBody->m_angularVelocity.setValue(0,0,0);
-		solverBody->m_externalForceImpulse.setValue(0,0,0);
-		solverBody->m_externalTorqueImpulse.setValue(0,0,0);
-	}
-
-
-}
-
-
-
-
-
+		solverBody->m_angularFactor.setValue(1, 1, 1);
+		solverBody->m_linearFactor.setValue(1, 1, 1);
+		solverBody->m_linearVelocity.setValue(0, 0, 0);
+		solverBody->m_angularVelocity.setValue(0, 0, 0);
+		solverBody->m_externalForceImpulse.setValue(0, 0, 0);
+		solverBody->m_externalTorqueImpulse.setValue(0, 0, 0);
+					}
+				}
 
-btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
+btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
 {
+	//printf("rel_vel =%f\n", rel_vel);
+	if (btFabs(rel_vel) < velocityThreshold)
+		return 0.;
+
 	btScalar rest = restitution * -rel_vel;
 	return rest;
 }
 
-
-
-void	btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
+void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj, btVector3& frictionDirection, int frictionMode)
 {
-
-
 	if (colObj && colObj->hasAnisotropicFriction(frictionMode))
 	{
 		// transform to local coordinates
@@ -524,21 +513,15 @@ void	btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionOb
 		// ... and transform it back to global coordinates
 		frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
 	}
-
 }
 
-
-
-
-void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int  solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
+void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
 {
-
-
 	btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
 	btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
 
 	btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
-	btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
+	btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
 
 	solverConstraint.m_solverBodyIdA = solverBodyIdA;
 	solverConstraint.m_solverBodyIdB = solverBodyIdB;
@@ -554,21 +537,23 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 		solverConstraint.m_contactNormal1 = normalAxis;
 		btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
 		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor();
-	}else
+		solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld() * ftorqueAxis1 * body0->getAngularFactor();
+	}
+	else
 	{
 		solverConstraint.m_contactNormal1.setZero();
 		solverConstraint.m_relpos1CrossNormal.setZero();
-		solverConstraint.m_angularComponentA .setZero();
+		solverConstraint.m_angularComponentA.setZero();
 	}
 
-	if (body1)
+	if (bodyA)
 	{
 		solverConstraint.m_contactNormal2 = -normalAxis;
 		btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
 		solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor();
-	} else
+		solverConstraint.m_angularComponentB = bodyA->getInvInertiaTensorWorld() * ftorqueAxis1 * bodyA->getAngularFactor();
+	}
+	else
 	{
 		solverConstraint.m_contactNormal2.setZero();
 		solverConstraint.m_relpos2CrossNormal.setZero();
@@ -581,60 +566,63 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 		btScalar denom1 = 0.f;
 		if (body0)
 		{
-			vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+			vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
 			denom0 = body0->getInvMass() + normalAxis.dot(vec);
 		}
-		if (body1)
+		if (bodyA)
 		{
-			vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
-			denom1 = body1->getInvMass() + normalAxis.dot(vec);
+			vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
+			denom1 = bodyA->getInvMass() + normalAxis.dot(vec);
 		}
-		btScalar denom = relaxation/(denom0+denom1);
+		btScalar denom = relaxation / (denom0 + denom1);
 		solverConstraint.m_jacDiagABInv = denom;
 	}
 
 	{
-
-
 		btScalar rel_vel;
-		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
-			+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
-		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
-			+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
+		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0 ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : btVector3(0, 0, 0));
+		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(bodyA ? solverBodyB.m_angularVelocity : btVector3(0, 0, 0));
 
-		rel_vel = vel1Dotn+vel2Dotn;
+		rel_vel = vel1Dotn + vel2Dotn;
 
-//		btScalar positionalError = 0.f;
+		//		btScalar positionalError = 0.f;
 
-		btScalar velocityError =  desiredVelocity - rel_vel;
+		btScalar velocityError = desiredVelocity - rel_vel;
 		btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
-		solverConstraint.m_rhs = velocityImpulse;
+
+		btScalar penetrationImpulse = btScalar(0);
+
+		if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
+		{
+			btScalar distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(normalAxis);
+			btScalar positionalError = -distance * infoGlobal.m_frictionERP / infoGlobal.m_timeStep;
+			penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
+		}
+
+		solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
 		solverConstraint.m_rhsPenetration = 0.f;
 		solverConstraint.m_cfm = cfmSlip;
 		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
 		solverConstraint.m_upperLimit = solverConstraint.m_friction;
-
 	}
 }
 
-btSolverConstraint&	btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
+btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
 {
 	btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
 	solverConstraint.m_frictionIndex = frictionIndex;
 	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
-							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
+		colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
 
-
-void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int  solverBodyIdB,
-									btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
-									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
-									btScalar desiredVelocity, btScalar cfmSlip)
+void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB,
+	btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
+	btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
+	btScalar desiredVelocity, btScalar cfmSlip)
 
 {
-	btVector3 normalAxis(0,0,0);
-
+	btVector3 normalAxis(0, 0, 0);
 
 	solverConstraint.m_contactNormal1 = normalAxis;
 	solverConstraint.m_contactNormal2 = -normalAxis;
@@ -642,12 +630,12 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolv
 	btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
 
 	btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
-	btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
+	btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
 
 	solverConstraint.m_solverBodyIdA = solverBodyIdA;
 	solverConstraint.m_solverBodyIdB = solverBodyIdB;
 
-	solverConstraint.m_friction = cp.m_combinedRollingFriction;
+	solverConstraint.m_friction = combinedTorsionalFriction;
 	solverConstraint.m_originalContactPoint = 0;
 
 	solverConstraint.m_appliedImpulse = 0.f;
@@ -656,66 +644,117 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolv
 	{
 		btVector3 ftorqueAxis1 = -normalAxis1;
 		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
+		solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld() * ftorqueAxis1 * body0->getAngularFactor() : btVector3(0, 0, 0);
 	}
 	{
 		btVector3 ftorqueAxis1 = normalAxis1;
 		solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
+		solverConstraint.m_angularComponentB = bodyA ? bodyA->getInvInertiaTensorWorld() * ftorqueAxis1 * bodyA->getAngularFactor() : btVector3(0, 0, 0);
 	}
 
-
 	{
-		btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
-		btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
+		btVector3 iMJaA = body0 ? body0->getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3(0, 0, 0);
+		btVector3 iMJaB = bodyA ? bodyA->getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3(0, 0, 0);
 		btScalar sum = 0;
 		sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
 		sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
-		solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
+		solverConstraint.m_jacDiagABInv = btScalar(1.) / sum;
 	}
 
 	{
-
-
 		btScalar rel_vel;
-		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
-			+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
-		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
-			+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
+		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0 ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : btVector3(0, 0, 0));
+		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(bodyA ? solverBodyB.m_angularVelocity : btVector3(0, 0, 0));
 
-		rel_vel = vel1Dotn+vel2Dotn;
+		rel_vel = vel1Dotn + vel2Dotn;
 
-//		btScalar positionalError = 0.f;
+		//		btScalar positionalError = 0.f;
 
-		btSimdScalar velocityError =  desiredVelocity - rel_vel;
-		btSimdScalar	velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
+		btSimdScalar velocityError = desiredVelocity - rel_vel;
+		btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
 		solverConstraint.m_rhs = velocityImpulse;
 		solverConstraint.m_cfm = cfmSlip;
 		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
 		solverConstraint.m_upperLimit = solverConstraint.m_friction;
-
 	}
 }
 
-
-
-
-
-
-
-
-btSolverConstraint&	btSequentialImpulseConstraintSolver::addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
+btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
 {
 	btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
 	solverConstraint.m_frictionIndex = frictionIndex;
-	setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
-							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
+	setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction, rel_pos1, rel_pos2,
+		colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
 
-
-int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep)
+int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body, btScalar timeStep)
 {
+#if BT_THREADSAFE
+	int solverBodyId = -1;
+	const bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
+	const bool isStaticOrKinematic = body.isStaticOrKinematicObject();
+	const bool isKinematic = body.isKinematicObject();
+	if (isRigidBodyType && !isStaticOrKinematic)
+	{
+		// dynamic body
+		// Dynamic bodies can only be in one island, so it's safe to write to the companionId
+		solverBodyId = body.getCompanionId();
+		if (solverBodyId < 0)
+		{
+			solverBodyId = m_tmpSolverBodyPool.size();
+			btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+			initSolverBody(&solverBody, &body, timeStep);
+			body.setCompanionId(solverBodyId);
+		}
+	}
+	else if (isRigidBodyType && isKinematic)
+	{
+		//
+		// NOTE: must test for kinematic before static because some kinematic objects also
+		//   identify as "static"
+		//
+		// Kinematic bodies can be in multiple islands at once, so it is a
+		// race condition to write to them, so we use an alternate method
+		// to record the solverBodyId
+		int uniqueId = body.getWorldArrayIndex();
+		const int INVALID_SOLVER_BODY_ID = -1;
+		if (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size())
+		{
+			m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
+		}
+		solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId];
+		// if no table entry yet,
+		if (solverBodyId == INVALID_SOLVER_BODY_ID)
+		{
+			// create a table entry for this body
+			solverBodyId = m_tmpSolverBodyPool.size();
+			btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+			initSolverBody(&solverBody, &body, timeStep);
+			m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId] = solverBodyId;
+		}
+	}
+	else
+	{
+		bool isMultiBodyType = (body.getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK);
+		// Incorrectly set collision object flags can degrade performance in various ways.
+		if (!isMultiBodyType)
+		{
+			btAssert(body.isStaticOrKinematicObject());
+		}
+		//it could be a multibody link collider
+		// all fixed bodies (inf mass) get mapped to a single solver id
+		if (m_fixedBodyId < 0)
+		{
+			m_fixedBodyId = m_tmpSolverBodyPool.size();
+			btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+			initSolverBody(&fixedBody, 0, timeStep);
+		}
+		solverBodyId = m_fixedBodyId;
+	}
+	btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size());
+	return solverBodyId;
+#else   // BT_THREADSAFE
 
 	int solverBodyIdA = -1;
 
@@ -723,8 +762,9 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 	{
 		//body has already been converted
 		solverBodyIdA = body.getCompanionId();
-        btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
-	} else
+		btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
+	}
+	else
 	{
 		btRigidBody* rb = btRigidBody::upcast(&body);
 		//convert both active and kinematic objects (for their velocity)
@@ -732,273 +772,253 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 		{
 			solverBodyIdA = m_tmpSolverBodyPool.size();
 			btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-			initSolverBody(&solverBody,&body,timeStep);
+			initSolverBody(&solverBody, &body, timeStep);
 			body.setCompanionId(solverBodyIdA);
-		} else
+		}
+		else
 		{
-
-			if (m_fixedBodyId<0)
+			if (m_fixedBodyId < 0)
 			{
 				m_fixedBodyId = m_tmpSolverBodyPool.size();
 				btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
-				initSolverBody(&fixedBody,0,timeStep);
+				initSolverBody(&fixedBody, 0, timeStep);
 			}
 			return m_fixedBodyId;
-//			return 0;//assume first one is a fixed solver body
+			//			return 0;//assume first one is a fixed solver body
 		}
 	}
 
 	return solverBodyIdA;
-
+#endif  // BT_THREADSAFE
 }
 #include <stdio.h>
 
-
 void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
-																 int solverBodyIdA, int solverBodyIdB,
-																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
-																 btScalar& relaxation,
-																 const btVector3& rel_pos1, const btVector3& rel_pos2)
+	int solverBodyIdA, int solverBodyIdB,
+	btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
+	btScalar& relaxation,
+	const btVector3& rel_pos1, const btVector3& rel_pos2)
 {
+	//	const btVector3& pos1 = cp.getPositionWorldOnA();
+	//	const btVector3& pos2 = cp.getPositionWorldOnB();
 
-		//	const btVector3& pos1 = cp.getPositionWorldOnA();
-		//	const btVector3& pos2 = cp.getPositionWorldOnB();
-
-			btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
-			btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
-
-			btRigidBody* rb0 = bodyA->m_originalBody;
-			btRigidBody* rb1 = bodyB->m_originalBody;
-
-//			btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
-//			btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
-			//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
-			//rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
-
-			relaxation = 1.f;
-
-			btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
-			solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
-			btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
-			solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
-
-				{
-#ifdef COMPUTE_IMPULSE_DENOM
-					btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
-					btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
-#else
-					btVector3 vec;
-					btScalar denom0 = 0.f;
-					btScalar denom1 = 0.f;
-					if (rb0)
-					{
-						vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
-						denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
-					}
-					if (rb1)
-					{
-						vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
-						denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
-					}
-#endif //COMPUTE_IMPULSE_DENOM
-
-					btScalar denom = relaxation/(denom0+denom1);
-					solverConstraint.m_jacDiagABInv = denom;
-				}
-
-				if (rb0)
-				{
-					solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
-					solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-				} else
-				{
-					solverConstraint.m_contactNormal1.setZero();
-					solverConstraint.m_relpos1CrossNormal.setZero();
-				}
-				if (rb1)
-				{
-					solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
-					solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
-				}else
-				{
-					solverConstraint.m_contactNormal2.setZero();
-					solverConstraint.m_relpos2CrossNormal.setZero();
-				}
-
-				btScalar restitution = 0.f;
-				btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
-
-				{
-					btVector3 vel1,vel2;
+	btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
+	btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
 
-					vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
-					vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
+	btRigidBody* rb0 = bodyA->m_originalBody;
+	btRigidBody* rb1 = bodyB->m_originalBody;
 
-	//			btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
-					btVector3 vel  = vel1 - vel2;
-					btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
+	//			btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
+	//			btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
+	//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
+	//rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
 
+	relaxation = infoGlobal.m_sor;
+	btScalar invTimeStep = btScalar(1) / infoGlobal.m_timeStep;
 
+	//cfm = 1 /       ( dt * kp + kd )
+	//erp = dt * kp / ( dt * kp + kd )
 
-					solverConstraint.m_friction = cp.m_combinedFriction;
+	btScalar cfm = infoGlobal.m_globalCfm;
+	btScalar erp = infoGlobal.m_erp2;
 
+	if ((cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP))
+	{
+		if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM)
+			cfm = cp.m_contactCFM;
+		if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP)
+			erp = cp.m_contactERP;
+	}
+	else
+	{
+		if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
+		{
+			btScalar denom = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1);
+			if (denom < SIMD_EPSILON)
+			{
+				denom = SIMD_EPSILON;
+			}
+			cfm = btScalar(1) / denom;
+			erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
+		}
+	}
 
-					restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
-					if (restitution <= btScalar(0.))
-					{
-						restitution = 0.f;
-					};
-				}
+	cfm *= invTimeStep;
 
+	btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+	solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
+	btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
+	solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
 
-				///warm starting (or zero if disabled)
-				if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-				{
-					solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-					if (rb0)
-						bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
-					if (rb1)
-						bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
-				} else
-				{
-					solverConstraint.m_appliedImpulse = 0.f;
-				}
+	{
+#ifdef COMPUTE_IMPULSE_DENOM
+		btScalar denom0 = rb0->computeImpulseDenominator(pos1, cp.m_normalWorldOnB);
+		btScalar denom1 = rb1->computeImpulseDenominator(pos2, cp.m_normalWorldOnB);
+#else
+		btVector3 vec;
+		btScalar denom0 = 0.f;
+		btScalar denom1 = 0.f;
+		if (rb0)
+		{
+			vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
+			denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
+		}
+		if (rb1)
+		{
+			vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
+			denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
+		}
+#endif  //COMPUTE_IMPULSE_DENOM
 
-				solverConstraint.m_appliedPushImpulse = 0.f;
+		btScalar denom = relaxation / (denom0 + denom1 + cfm);
+		solverConstraint.m_jacDiagABInv = denom;
+	}
 
-				{
+	if (rb0)
+	{
+		solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
+		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+	}
+	else
+	{
+		solverConstraint.m_contactNormal1.setZero();
+		solverConstraint.m_relpos1CrossNormal.setZero();
+	}
+	if (rb1)
+	{
+		solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
+		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+	}
+	else
+	{
+		solverConstraint.m_contactNormal2.setZero();
+		solverConstraint.m_relpos2CrossNormal.setZero();
+	}
 
-					btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse: btVector3(0,0,0);
-					btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0);
-					btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0);
-					btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0);
+	btScalar restitution = 0.f;
+	btScalar penetration = cp.getDistance() + infoGlobal.m_linearSlop;
 
+	{
+		btVector3 vel1, vel2;
 
-					btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA)
-						+ solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA);
-					btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB)
-						+ solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB);
-					btScalar rel_vel = vel1Dotn+vel2Dotn;
+		vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0, 0, 0);
+		vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0, 0, 0);
 
-					btScalar positionalError = 0.f;
-					btScalar	velocityError = restitution - rel_vel;// * damping;
+		//			btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
+		btVector3 vel = vel1 - vel2;
+		btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
 
+		solverConstraint.m_friction = cp.m_combinedFriction;
 
-					btScalar erp = infoGlobal.m_erp2;
-					if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-					{
-						erp = infoGlobal.m_erp;
-					}
+		restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
+		if (restitution <= btScalar(0.))
+		{
+			restitution = 0.f;
+		};
+	}
 
-					if (penetration>0)
-					{
-						positionalError = 0;
+	///warm starting (or zero if disabled)
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	{
+		solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+		if (rb0)
+			bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
+		if (rb1)
+			bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
+	}
+	else
+	{
+		solverConstraint.m_appliedImpulse = 0.f;
+	}
 
-						velocityError -= penetration / infoGlobal.m_timeStep;
-					} else
-					{
-						positionalError = -penetration * erp/infoGlobal.m_timeStep;
-					}
+	solverConstraint.m_appliedPushImpulse = 0.f;
 
-					btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
-					btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+	{
+		btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse : btVector3(0, 0, 0);
+		btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse : btVector3(0, 0, 0);
+		btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse : btVector3(0, 0, 0);
+		btVector3 externalTorqueImpulseB = bodyB->m_originalBody ? bodyB->m_externalTorqueImpulse : btVector3(0, 0, 0);
 
-					if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-					{
-						//combine position and velocity into rhs
-						solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
-						solverConstraint.m_rhsPenetration = 0.f;
+		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity + externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity + externalTorqueImpulseA);
+		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity + externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity + externalTorqueImpulseB);
+		btScalar rel_vel = vel1Dotn + vel2Dotn;
 
-					} else
-					{
-						//split position and velocity into rhs and m_rhsPenetration
-						solverConstraint.m_rhs = velocityImpulse;
-						solverConstraint.m_rhsPenetration = penetrationImpulse;
-					}
-					solverConstraint.m_cfm = 0.f;
-					solverConstraint.m_lowerLimit = 0;
-					solverConstraint.m_upperLimit = 1e10f;
-				}
+		btScalar positionalError = 0.f;
+		btScalar velocityError = restitution - rel_vel;  // * damping;
 
+		if (penetration > 0)
+		{
+			positionalError = 0;
 
+			velocityError -= penetration * invTimeStep;
+		}
+		else
+		{
+			positionalError = -penetration * erp * invTimeStep;
+		}
 
+		btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
+		btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
 
+		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		{
+			//combine position and velocity into rhs
+			solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;  //-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
+			solverConstraint.m_rhsPenetration = 0.f;
+		}
+		else
+		{
+			//split position and velocity into rhs and m_rhsPenetration
+			solverConstraint.m_rhs = velocityImpulse;
+			solverConstraint.m_rhsPenetration = penetrationImpulse;
+		}
+		solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv;
+		solverConstraint.m_lowerLimit = 0;
+		solverConstraint.m_upperLimit = 1e10f;
+	}
 }
 
-
-
-void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
-																		int solverBodyIdA, int solverBodyIdB,
-																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
+void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse(btSolverConstraint& solverConstraint,
+	int solverBodyIdA, int solverBodyIdB,
+	btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
 {
-
-	btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
-	btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
-
-	btRigidBody* rb0 = bodyA->m_originalBody;
-	btRigidBody* rb1 = bodyB->m_originalBody;
-
 	{
 		btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
-		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-		{
-			frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
-			if (rb0)
-				bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
-			if (rb1)
-				bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
-		} else
-		{
-			frictionConstraint1.m_appliedImpulse = 0.f;
-		}
+		
+		frictionConstraint1.m_appliedImpulse = 0.f;
 	}
 
 	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 	{
-		btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
-		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-		{
-			frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2  * infoGlobal.m_warmstartingFactor;
-			if (rb0)
-				bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
-			if (rb1)
-				bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
-		} else
-		{
-			frictionConstraint2.m_appliedImpulse = 0.f;
-		}
+		btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
+		
+		frictionConstraint2.m_appliedImpulse = 0.f;
 	}
 }
 
-
-
-
-void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
+void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal)
 {
-	btCollisionObject* colObj0=0,*colObj1=0;
+	btCollisionObject *colObj0 = 0, *colObj1 = 0;
 
 	colObj0 = (btCollisionObject*)manifold->getBody0();
 	colObj1 = (btCollisionObject*)manifold->getBody1();
 
-	int solverBodyIdA = getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
-	int solverBodyIdB = getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
+	int solverBodyIdA = getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
+	int solverBodyIdB = getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
 
-//	btRigidBody* bodyA = btRigidBody::upcast(colObj0);
-//	btRigidBody* bodyB = btRigidBody::upcast(colObj1);
+	//	btRigidBody* bodyA = btRigidBody::upcast(colObj0);
+	//	btRigidBody* bodyB = btRigidBody::upcast(colObj1);
 
 	btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
 	btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
 
-
-
 	///avoid collision response between two static objects
 	if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
 		return;
 
-	int rollingFriction=1;
-	for (int j=0;j<manifold->getNumContacts();j++)
+	int rollingFriction = 1;
+	for (int j = 0; j < manifold->getNumContacts(); j++)
 	{
-
 		btManifoldPoint& cp = manifold->getContactPoint(j);
 
 		if (cp.getDistance() <= manifold->getContactProcessingThreshold())
@@ -1007,11 +1027,8 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			btVector3 rel_pos2;
 			btScalar relaxation;
 
-
 			int frictionIndex = m_tmpSolverContactConstraintPool.size();
 			btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
-			btRigidBody* rb0 = btRigidBody::upcast(colObj0);
-			btRigidBody* rb1 = btRigidBody::upcast(colObj1);
 			solverConstraint.m_solverBodyIdA = solverBodyIdA;
 			solverConstraint.m_solverBodyIdB = solverBodyIdB;
 
@@ -1023,61 +1040,42 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
 			rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
 
-			btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
-			btVector3 vel2;// = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
+			btVector3 vel1;
+			btVector3 vel2;
 
-			solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1);
-			solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 );
+			solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1, vel1);
+			solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2, vel2);
 
-			btVector3 vel  = vel1 - vel2;
+			btVector3 vel = vel1 - vel2;
 			btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
 
 			setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
 
-
-
-//			const btVector3& pos1 = cp.getPositionWorldOnA();
-//			const btVector3& pos2 = cp.getPositionWorldOnB();
-
 			/////setup the friction constraints
 
 			solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
 
-			btVector3 angVelA(0,0,0),angVelB(0,0,0);
-			if (rb0)
-				angVelA = rb0->getAngularVelocity();
-			if (rb1)
-				angVelB = rb1->getAngularVelocity();
-			btVector3 relAngVel = angVelB-angVelA;
-
-			if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
+			if ((cp.m_combinedRollingFriction > 0.f) && (rollingFriction > 0))
 			{
-				//only a single rollingFriction per manifold
-				rollingFriction--;
-				if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
-				{
-					relAngVel.normalize();
-					applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					if (relAngVel.length()>0.001)
-						addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
-				} else
 				{
-					addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-					btVector3 axis0,axis1;
-					btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
-					applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					if (axis0.length()>0.001)
-						addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-					if (axis1.length()>0.001)
-						addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
+					addTorsionalFrictionConstraint(cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.m_combinedSpinningFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+					btVector3 axis0, axis1;
+					btPlaneSpace1(cp.m_normalWorldOnB, axis0, axis1);
+					axis0.normalize();
+					axis1.normalize();
+
+					applyAnisotropicFriction(colObj0, axis0, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj1, axis0, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj0, axis1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj1, axis1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					if (axis0.length() > 0.001)
+						addTorsionalFrictionConstraint(axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp,
+							cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+					if (axis1.length() > 0.001)
+						addTorsionalFrictionConstraint(axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp,
+							cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+					}
 				}
-			}
 
 			///Bullet has several options to set the friction directions
 			///By default, each contact has only a single friction direction that is recomputed automatically very frame
@@ -1090,408 +1088,409 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
 			///
 			///The user can manually override the friction directions for certain contacts using a contact callback,
-			///and set the cp.m_lateralFrictionInitialized to true
+			///and use contactPoint.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
 			///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
 			///this will give a conveyor belt effect
 			///
-			if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+
+			if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
 			{
 				cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
 				btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
 				if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
 				{
-					cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
-					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+					cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel);
+					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
 
-					if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
 						cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
-						cp.m_lateralFrictionDir2.normalize();//??
-						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+						cp.m_lateralFrictionDir2.normalize();  //??
+						applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+						applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+						addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
 					}
-
-				} else
+				}
+				else
 				{
-					btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+					btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2);
 
-					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
-						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+						applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+						applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+						addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
 					}
 
-
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
 					{
-						cp.m_lateralFrictionInitialized = true;
+						cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
 					}
 				}
-
-			} else
+			}
+			else
 			{
-				addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
+				addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
 
 				if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-					addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
-
+					addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
+				}
+			setFrictionConstraintImpulse(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
 			}
-			setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
-
-
-
-
 		}
 	}
-}
 
-void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
+void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
 {
 	int i;
 	btPersistentManifold* manifold = 0;
-//			btCollisionObject* colObj0=0,*colObj1=0;
-
+	//			btCollisionObject* colObj0=0,*colObj1=0;
 
-	for (i=0;i<numManifolds;i++)
+	for (i = 0; i < numManifolds; i++)
 	{
 		manifold = manifoldPtr[i];
-		convertContact(manifold,infoGlobal);
+		convertContact(manifold, infoGlobal);
 	}
 }
 
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* currentConstraintRow,
+	btTypedConstraint* constraint,
+	const btTypedConstraint::btConstraintInfo1& info1,
+	int solverBodyIdA,
+	int solverBodyIdB,
+	const btContactSolverInfo& infoGlobal)
 {
-	m_fixedBodyId = -1;
-	BT_PROFILE("solveGroupCacheFriendlySetup");
-	(void)debugDrawer;
+	const btRigidBody& rbA = constraint->getRigidBodyA();
+	const btRigidBody& rbB = constraint->getRigidBodyB();
 
-	m_maxOverrideNumSolverIterations = 0;
+	const btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
+	const btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
 
-#ifdef BT_ADDITIONAL_DEBUG
-	 //make sure that dynamic bodies exist for all (enabled) constraints
-	for (int i=0;i<numConstraints;i++)
+	int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
+	if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations)
+		m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
+
+	for (int j = 0; j < info1.m_numConstraintRows; j++)
 	{
-		btTypedConstraint* constraint = constraints[i];
-		if (constraint->isEnabled())
+		memset(&currentConstraintRow[j], 0, sizeof(btSolverConstraint));
+		currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
+		currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
+		currentConstraintRow[j].m_appliedImpulse = 0.f;
+		currentConstraintRow[j].m_appliedPushImpulse = 0.f;
+		currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
+		currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
+		currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
+	}
+
+	// these vectors are already cleared in initSolverBody, no need to redundantly clear again
+	btAssert(bodyAPtr->getDeltaLinearVelocity().isZero());
+	btAssert(bodyAPtr->getDeltaAngularVelocity().isZero());
+	btAssert(bodyAPtr->getPushVelocity().isZero());
+	btAssert(bodyAPtr->getTurnVelocity().isZero());
+	btAssert(bodyBPtr->getDeltaLinearVelocity().isZero());
+	btAssert(bodyBPtr->getDeltaAngularVelocity().isZero());
+	btAssert(bodyBPtr->getPushVelocity().isZero());
+	btAssert(bodyBPtr->getTurnVelocity().isZero());
+	//bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
+	//bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
+	//bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
+	//bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
+	//bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
+	//bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
+	//bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
+	//bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
+
+	btTypedConstraint::btConstraintInfo2 info2;
+	info2.fps = 1.f / infoGlobal.m_timeStep;
+	info2.erp = infoGlobal.m_erp;
+	info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
+	info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
+	info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
+	info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
+	info2.rowskip = sizeof(btSolverConstraint) / sizeof(btScalar);  //check this
+																	///the size of btSolverConstraint needs be a multiple of btScalar
+	btAssert(info2.rowskip * sizeof(btScalar) == sizeof(btSolverConstraint));
+	info2.m_constraintError = &currentConstraintRow->m_rhs;
+	currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
+	info2.m_damping = infoGlobal.m_damping;
+	info2.cfm = &currentConstraintRow->m_cfm;
+	info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
+	info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
+	info2.m_numIterations = infoGlobal.m_numIterations;
+	constraint->getInfo2(&info2);
+
+	///finalize the constraint setup
+	for (int j = 0; j < info1.m_numConstraintRows; j++)
+	{
+		btSolverConstraint& solverConstraint = currentConstraintRow[j];
+
+		if (solverConstraint.m_upperLimit >= constraint->getBreakingImpulseThreshold())
 		{
-			if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
-			{
-				bool found=false;
-				for (int b=0;b<numBodies;b++)
-				{
+			solverConstraint.m_upperLimit = constraint->getBreakingImpulseThreshold();
+		}
 
-					if (&constraint->getRigidBodyA()==bodies[b])
-					{
-						found = true;
-						break;
-					}
-				}
-				btAssert(found);
-			}
-			if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
-			{
-				bool found=false;
-				for (int b=0;b<numBodies;b++)
-				{
-					if (&constraint->getRigidBodyB()==bodies[b])
-					{
-						found = true;
-						break;
-					}
-				}
-				btAssert(found);
-			}
+		if (solverConstraint.m_lowerLimit <= -constraint->getBreakingImpulseThreshold())
+		{
+			solverConstraint.m_lowerLimit = -constraint->getBreakingImpulseThreshold();
+		}
+
+		solverConstraint.m_originalContactPoint = constraint;
+
+		{
+			const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
+			solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld() * ftorqueAxis1 * constraint->getRigidBodyA().getAngularFactor();
+		}
+		{
+			const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
+			solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld() * ftorqueAxis2 * constraint->getRigidBodyB().getAngularFactor();
+		}
+
+		{
+			btVector3 iMJlA = solverConstraint.m_contactNormal1 * rbA.getInvMass();
+			btVector3 iMJaA = rbA.getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal;
+			btVector3 iMJlB = solverConstraint.m_contactNormal2 * rbB.getInvMass();  //sign of normal?
+			btVector3 iMJaB = rbB.getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal;
+
+			btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
+			sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
+			sum += iMJlB.dot(solverConstraint.m_contactNormal2);
+			sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
+			btScalar fsum = btFabs(sum);
+			btAssert(fsum > SIMD_EPSILON);
+			btScalar sorRelaxation = 1.f;  //todo: get from globalInfo?
+			solverConstraint.m_jacDiagABInv = fsum > SIMD_EPSILON ? sorRelaxation / sum : 0.f;
 		}
+
+		{
+			btScalar rel_vel;
+			btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0, 0, 0);
+			btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0, 0, 0);
+
+			btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0, 0, 0);
+			btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalTorqueImpulse : btVector3(0, 0, 0);
+
+			btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity() + externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity() + externalTorqueImpulseA);
+
+			btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity() + externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity() + externalTorqueImpulseB);
+
+			rel_vel = vel1Dotn + vel2Dotn;
+			btScalar restitution = 0.f;
+			btScalar positionalError = solverConstraint.m_rhs;  //already filled in by getConstraintInfo2
+			btScalar velocityError = restitution - rel_vel * info2.m_damping;
+			btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
+			btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
+			solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
+			solverConstraint.m_appliedImpulse = 0.f;
+		}
+	}
+}
+
+void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("convertJoints");
+	for (int j = 0; j < numConstraints; j++)
+	{
+		btTypedConstraint* constraint = constraints[j];
+		constraint->buildJacobian();
+		constraint->internalSetAppliedImpulse(0.0f);
 	}
-    //make sure that dynamic bodies exist for all contact manifolds
-    for (int i=0;i<numManifolds;i++)
-    {
-        if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
-        {
-            bool found=false;
-            for (int b=0;b<numBodies;b++)
-            {
-
-                if (manifoldPtr[i]->getBody0()==bodies[b])
-                {
-                    found = true;
-                    break;
-                }
-            }
-            btAssert(found);
-        }
-        if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
-        {
-            bool found=false;
-            for (int b=0;b<numBodies;b++)
-            {
-                if (manifoldPtr[i]->getBody1()==bodies[b])
-                {
-                    found = true;
-                    break;
-                }
-            }
-            btAssert(found);
-        }
-    }
-#endif //BT_ADDITIONAL_DEBUG
 
+	int totalNumRows = 0;
 
+	m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
+	//calculate the total number of contraint rows
+	for (int i = 0; i < numConstraints; i++)
+	{
+		btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+		btJointFeedback* fb = constraints[i]->getJointFeedback();
+		if (fb)
+		{
+			fb->m_appliedForceBodyA.setZero();
+			fb->m_appliedTorqueBodyA.setZero();
+			fb->m_appliedForceBodyB.setZero();
+			fb->m_appliedTorqueBodyB.setZero();
+		}
+
+		if (constraints[i]->isEnabled())
+		{
+			constraints[i]->getInfo1(&info1);
+		}
+		else
+		{
+			info1.m_numConstraintRows = 0;
+			info1.nub = 0;
+		}
+		totalNumRows += info1.m_numConstraintRows;
+	}
+	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
+
+	///setup the btSolverConstraints
+	int currentRow = 0;
+
+	for (int i = 0; i < numConstraints; i++)
+	{
+		const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+
+		if (info1.m_numConstraintRows)
+		{
+			btAssert(currentRow < totalNumRows);
+
+			btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
+			btTypedConstraint* constraint = constraints[i];
+			btRigidBody& rbA = constraint->getRigidBodyA();
+			btRigidBody& rbB = constraint->getRigidBodyB();
+
+			int solverBodyIdA = getOrInitSolverBody(rbA, infoGlobal.m_timeStep);
+			int solverBodyIdB = getOrInitSolverBody(rbB, infoGlobal.m_timeStep);
+
+			convertJoint(currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal);
+		}
+		currentRow += info1.m_numConstraintRows;
+	}
+}
+
+void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("convertBodies");
 	for (int i = 0; i < numBodies; i++)
 	{
 		bodies[i]->setCompanionId(-1);
 	}
+#if BT_THREADSAFE
+	m_kinematicBodyUniqueIdToSolverBodyTable.resize(0);
+#endif  // BT_THREADSAFE
 
-
-	m_tmpSolverBodyPool.reserve(numBodies+1);
+	m_tmpSolverBodyPool.reserve(numBodies + 1);
 	m_tmpSolverBodyPool.resize(0);
 
 	//btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
-    //initSolverBody(&fixedBody,0);
+	//initSolverBody(&fixedBody,0);
 
-	//convert all bodies
-
-
-	for (int i=0;i<numBodies;i++)
+	for (int i = 0; i < numBodies; i++)
 	{
-		int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
+		int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
 
 		btRigidBody* body = btRigidBody::upcast(bodies[i]);
 		if (body && body->getInvMass())
 		{
 			btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
-			btVector3 gyroForce (0,0,0);
-			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
+			btVector3 gyroForce(0, 0, 0);
+			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
 			{
 				gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
-				solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
+				solverBody.m_externalTorqueImpulse -= gyroForce * body->getInvInertiaTensorWorld() * infoGlobal.m_timeStep;
 			}
-			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
+			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
 			{
 				gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
 				solverBody.m_externalTorqueImpulse += gyroForce;
 			}
-			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
+			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
 			{
 				gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
 				solverBody.m_externalTorqueImpulse += gyroForce;
-
 			}
-			
-
 		}
 	}
+}
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+{
+	m_fixedBodyId = -1;
+	BT_PROFILE("solveGroupCacheFriendlySetup");
+	(void)debugDrawer;
 
-	if (1)
+	// if solver mode has changed,
+	if (infoGlobal.m_solverMode != m_cachedSolverMode)
 	{
-		int j;
-		for (j=0;j<numConstraints;j++)
-		{
-			btTypedConstraint* constraint = constraints[j];
-			constraint->buildJacobian();
-			constraint->internalSetAppliedImpulse(0.0f);
-		}
+		// update solver functions to use SIMD or non-SIMD
+		bool useSimd = !!(infoGlobal.m_solverMode & SOLVER_SIMD);
+		setupSolverFunctions(useSimd);
+		m_cachedSolverMode = infoGlobal.m_solverMode;
 	}
+	m_maxOverrideNumSolverIterations = 0;
 
-	//btRigidBody* rb0=0,*rb1=0;
-
-	//if (1)
+#ifdef BT_ADDITIONAL_DEBUG
+	//make sure that dynamic bodies exist for all (enabled) constraints
+	for (int i = 0; i < numConstraints; i++)
 	{
+		btTypedConstraint* constraint = constraints[i];
+		if (constraint->isEnabled())
 		{
-
-			int totalNumRows = 0;
-			int i;
-
-			m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
-			//calculate the total number of contraint rows
-			for (i=0;i<numConstraints;i++)
+			if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
 			{
-				btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
-				btJointFeedback* fb = constraints[i]->getJointFeedback();
-				if (fb)
+				bool found = false;
+				for (int b = 0; b < numBodies; b++)
 				{
-					fb->m_appliedForceBodyA.setZero();
-					fb->m_appliedTorqueBodyA.setZero();
-					fb->m_appliedForceBodyB.setZero();
-					fb->m_appliedTorqueBodyB.setZero();
+					if (&constraint->getRigidBodyA() == bodies[b])
+					{
+						found = true;
+						break;
+					}
 				}
-
-				if (constraints[i]->isEnabled())
+				btAssert(found);
+			}
+			if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
+			{
+				bool found = false;
+				for (int b = 0; b < numBodies; b++)
 				{
+					if (&constraint->getRigidBodyB() == bodies[b])
+					{
+						found = true;
+						break;
+					}
 				}
-				if (constraints[i]->isEnabled())
-				{
-					constraints[i]->getInfo1(&info1);
-				} else
+				btAssert(found);
+			}
+		}
+	}
+	//make sure that dynamic bodies exist for all contact manifolds
+	for (int i = 0; i < numManifolds; i++)
+	{
+		if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
+		{
+			bool found = false;
+			for (int b = 0; b < numBodies; b++)
+			{
+				if (manifoldPtr[i]->getBody0() == bodies[b])
 				{
-					info1.m_numConstraintRows = 0;
-					info1.nub = 0;
+					found = true;
+					break;
 				}
-				totalNumRows += info1.m_numConstraintRows;
 			}
-			m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
-
-
-			///setup the btSolverConstraints
-			int currentRow = 0;
-
-			for (i=0;i<numConstraints;i++)
+			btAssert(found);
+		}
+		if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
+		{
+			bool found = false;
+			for (int b = 0; b < numBodies; b++)
 			{
-				const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
-
-				if (info1.m_numConstraintRows)
+				if (manifoldPtr[i]->getBody1() == bodies[b])
 				{
-					btAssert(currentRow<totalNumRows);
-
-					btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
-					btTypedConstraint* constraint = constraints[i];
-					btRigidBody& rbA = constraint->getRigidBodyA();
-					btRigidBody& rbB = constraint->getRigidBodyB();
-
-					int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep);
-                    int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep);
-
-                    btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
-                    btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
-
-
-
-
-					int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
-					if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
-						m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
-
-
-					int j;
-					for ( j=0;j<info1.m_numConstraintRows;j++)
-					{
-						memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
-						currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
-						currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
-						currentConstraintRow[j].m_appliedImpulse = 0.f;
-						currentConstraintRow[j].m_appliedPushImpulse = 0.f;
-						currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
-						currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
-						currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
-					}
-
-					bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
-					bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
-					bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
-					bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
-					bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
-					bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
-					bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
-					bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
-
-
-					btTypedConstraint::btConstraintInfo2 info2;
-					info2.fps = 1.f/infoGlobal.m_timeStep;
-					info2.erp = infoGlobal.m_erp;
-					info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
-					info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
-					info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
-					info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
-					info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
-					///the size of btSolverConstraint needs be a multiple of btScalar
-		            btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
-					info2.m_constraintError = &currentConstraintRow->m_rhs;
-					currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
-					info2.m_damping = infoGlobal.m_damping;
-					info2.cfm = &currentConstraintRow->m_cfm;
-					info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
-					info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
-					info2.m_numIterations = infoGlobal.m_numIterations;
-					constraints[i]->getInfo2(&info2);
-
-					///finalize the constraint setup
-					for ( j=0;j<info1.m_numConstraintRows;j++)
-					{
-						btSolverConstraint& solverConstraint = currentConstraintRow[j];
-
-						if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold())
-						{
-							solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
-						}
-
-						if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold())
-						{
-							solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
-						}
-
-						solverConstraint.m_originalContactPoint = constraint;
-
-						{
-							const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
-							solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
-						}
-						{
-							const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
-							solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
-						}
-
-						{
-							btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass();
-							btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
-							btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal?
-							btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
-
-							btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
-							sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
-							sum += iMJlB.dot(solverConstraint.m_contactNormal2);
-							sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
-							btScalar fsum = btFabs(sum);
-							btAssert(fsum > SIMD_EPSILON);
-							solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f;
-						}
-
-
-
-						{
-							btScalar rel_vel;
-							btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0);
-							btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0);
-
-							btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0);
-							btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0);
-
-							btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA)
-												+ solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
-
-							btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB)
-																+ solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB);
-
-							rel_vel = vel1Dotn+vel2Dotn;
-							btScalar restitution = 0.f;
-							btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
-							btScalar	velocityError = restitution - rel_vel * info2.m_damping;
-							btScalar	penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
-							btScalar	velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
-							solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
-							solverConstraint.m_appliedImpulse = 0.f;
-
-
-						}
-					}
+					found = true;
+					break;
 				}
-				currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
 			}
+			btAssert(found);
 		}
+	}
+#endif  //BT_ADDITIONAL_DEBUG
 
-		convertContacts(manifoldPtr,numManifolds,infoGlobal);
+	//convert all bodies
+	convertBodies(bodies, numBodies, infoGlobal);
 
-	}
+	convertJoints(constraints, numConstraints, infoGlobal);
 
-//	btContactSolverInfo info = infoGlobal;
+	convertContacts(manifoldPtr, numManifolds, infoGlobal);
 
+	//	btContactSolverInfo info = infoGlobal;
 
 	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
 	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
@@ -1500,34 +1499,34 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 	///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
 	m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
 	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-		m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2);
+		m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2);
 	else
 		m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
 
 	m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
 	{
 		int i;
-		for (i=0;i<numNonContactPool;i++)
+		for (i = 0; i < numNonContactPool; i++)
 		{
 			m_orderNonContactConstraintPool[i] = i;
 		}
-		for (i=0;i<numConstraintPool;i++)
+		for (i = 0; i < numConstraintPool; i++)
 		{
 			m_orderTmpConstraintPool[i] = i;
 		}
-		for (i=0;i<numFrictionPool;i++)
+		for (i = 0; i < numFrictionPool; i++)
 		{
 			m_orderFrictionConstraintPool[i] = i;
 		}
 	}
 
 	return 0.f;
-
 }
 
-
-btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
+btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/)
 {
+	BT_PROFILE("solveSingleIteration");
+	btScalar leastSquaresResidual = 0.f;
 
 	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
 	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
@@ -1535,29 +1534,31 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 
 	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
 	{
-		if (1)			// uncomment this for a bit less random ((iteration & 7) == 0)
+		if (1)  // uncomment this for a bit less random ((iteration & 7) == 0)
 		{
-
-			for (int j=0; j<numNonContactPool; ++j) {
+			for (int j = 0; j < numNonContactPool; ++j)
+			{
 				int tmp = m_orderNonContactConstraintPool[j];
-				int swapi = btRandInt2(j+1);
+				int swapi = btRandInt2(j + 1);
 				m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
 				m_orderNonContactConstraintPool[swapi] = tmp;
 			}
 
 			//contact/friction constraints are not solved more than
-			if (iteration< infoGlobal.m_numIterations)
+			if (iteration < infoGlobal.m_numIterations)
 			{
-				for (int j=0; j<numConstraintPool; ++j) {
+				for (int j = 0; j < numConstraintPool; ++j)
+				{
 					int tmp = m_orderTmpConstraintPool[j];
-					int swapi = btRandInt2(j+1);
+					int swapi = btRandInt2(j + 1);
 					m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
 					m_orderTmpConstraintPool[swapi] = tmp;
 				}
 
-				for (int j=0; j<numFrictionPool; ++j) {
+				for (int j = 0; j < numFrictionPool; ++j)
+				{
 					int tmp = m_orderFrictionConstraintPool[j];
-					int swapi = btRandInt2(j+1);
+					int swapi = btRandInt2(j + 1);
 					m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
 					m_orderFrictionConstraintPool[swapi] = tmp;
 				}
@@ -1565,313 +1566,248 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 		}
 	}
 
-	if (infoGlobal.m_solverMode & SOLVER_SIMD)
+	///solve all joint constraints
+	for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
 	{
-		///solve all joint constraints, using SIMD, if available
-		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+		if (iteration < constraint.m_overrideNumSolverIterations)
 		{
-			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-			if (iteration < constraint.m_overrideNumSolverIterations)
-				resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+			btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint);
+			leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 		}
+	}
 
-		if (iteration< infoGlobal.m_numIterations)
+	if (iteration < infoGlobal.m_numIterations)
+	{
+		for (int j = 0; j < numConstraints; j++)
 		{
-			for (int j=0;j<numConstraints;j++)
+			if (constraints[j]->isEnabled())
 			{
-				if (constraints[j]->isEnabled())
-				{
-					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
-					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
-					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
-					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
-					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
-				}
+				int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(), infoGlobal.m_timeStep);
+				int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(), infoGlobal.m_timeStep);
+				btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+				btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+				constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
 			}
+		}
 
-			///solve all contact constraints using SIMD, if available
-			if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
-			{
-				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-				int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
-
-				for (int c=0;c<numPoolConstraints;c++)
-				{
-					btScalar totalImpulse =0;
-
-					{
-						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
-						resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-						totalImpulse = solveManifold.m_appliedImpulse;
-					}
-					bool applyFriction = true;
-					if (applyFriction)
-					{
-						{
-
-							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
-
-							if (totalImpulse>btScalar(0))
-							{
-								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-
-								resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-							}
-						}
-
-						if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
-						{
-
-							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
-
-							if (totalImpulse>btScalar(0))
-							{
-								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-
-								resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-							}
-						}
-					}
-				}
+		///solve all contact constraints
+		if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
+		{
+			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+			int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
 
-			}
-			else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
+			for (int c = 0; c < numPoolConstraints; c++)
 			{
-				//solve the friction constraints after all contact constraints, don't interleave them
-				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-				int j;
+				btScalar totalImpulse = 0;
 
-				for (j=0;j<numPoolConstraints;j++)
 				{
-					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-					resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
+					btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+					leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 
+					totalImpulse = solveManifold.m_appliedImpulse;
 				}
-
-
-
-				///solve all friction constraints, using SIMD, if available
-
-				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-				for (j=0;j<numFrictionPoolConstraints;j++)
+				bool applyFriction = true;
+				if (applyFriction)
 				{
-					btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-					btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-
-					if (totalImpulse>btScalar(0))
 					{
-						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-
-						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-					}
-				}
+						btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]];
 
+						if (totalImpulse > btScalar(0))
+						{
+							solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+							solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
 
-				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-				for (j=0;j<numRollingFrictionPoolConstraints;j++)
-				{
+							btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+							leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
+						}
+					}
 
-					btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
-					btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
-					if (totalImpulse>btScalar(0))
+					if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
 					{
-						btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
-						if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
-							rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+						btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]];
 
-						rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
-						rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+						if (totalImpulse > btScalar(0))
+						{
+							solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+							solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
 
-						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+							btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+							leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
+						}
 					}
 				}
-
-
 			}
 		}
-	} else
-	{
-		//non-SIMD version
-		///solve all joint constraints
-		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
-		{
-			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
-			if (iteration < constraint.m_overrideNumSolverIterations)
-				resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
-		}
-
-		if (iteration< infoGlobal.m_numIterations)
+		else  //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
 		{
-			for (int j=0;j<numConstraints;j++)
-			{
-				if (constraints[j]->isEnabled())
-				{
-					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
-					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
-					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
-					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
-					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
-				}
-			}
-			///solve all contact constraints
+			//solve the friction constraints after all contact constraints, don't interleave them
 			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-			for (int j=0;j<numPoolConstraints;j++)
+			int j;
+
+			for (j = 0; j < numPoolConstraints; j++)
 			{
 				const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-				resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+				btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+				leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 			}
+
 			///solve all friction constraints
+
 			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-			for (int j=0;j<numFrictionPoolConstraints;j++)
+			for (j = 0; j < numFrictionPoolConstraints; j++)
 			{
 				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
 				btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
 
-				if (totalImpulse>btScalar(0))
+				if (totalImpulse > btScalar(0))
 				{
-					solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-					solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+					solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+					solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
 
-					resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+					leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 				}
 			}
+		}
 
-			int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-			for (int j=0;j<numRollingFrictionPoolConstraints;j++)
+		int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+		for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
+		{
+			btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+			btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+			if (totalImpulse > btScalar(0))
 			{
-				btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
-				btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
-				if (totalImpulse>btScalar(0))
-				{
-					btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
-					if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
-						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+				btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
+				if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
+					rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
 
-					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
-					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+				rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+				rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
 
-					resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
-				}
+				btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
+				leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 			}
 		}
 	}
-	return 0.f;
+	return leastSquaresResidual;
 }
 
-
-void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
+	BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
 	int iteration;
 	if (infoGlobal.m_splitImpulse)
 	{
-		if (infoGlobal.m_solverMode & SOLVER_SIMD)
 		{
-			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+			for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
 			{
+				btScalar leastSquaresResidual = 0.f;
 				{
 					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
 					int j;
-					for (j=0;j<numPoolConstraints;j++)
+					for (j = 0; j < numPoolConstraints; j++)
 					{
 						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
 
-						resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+						leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 					}
 				}
-			}
-		}
-		else
-		{
-			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
-			{
+				if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
 				{
-					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-					int j;
-					for (j=0;j<numPoolConstraints;j++)
-					{
-						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-
-						resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-					}
+#ifdef VERBOSE_RESIDUAL_PRINTF
+					printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
+#endif
+					break;
 				}
 			}
 		}
 	}
 }
 
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	BT_PROFILE("solveGroupCacheFriendlyIterations");
 
 	{
 		///this is a special step to resolve penetrations (just for contacts)
-		solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+		solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
-		int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
+		int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
 
-		for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
-		//for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
+		for (int iteration = 0; iteration < maxIterations; iteration++)
+			//for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
 		{
-			solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
-		}
+			m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
+			if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
+			{
+#ifdef VERBOSE_RESIDUAL_PRINTF
+				printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
+#endif
+				m_analyticsData.m_numSolverCalls++;
+				m_analyticsData.m_numIterationsUsed = iteration+1;
+				m_analyticsData.m_islandId = -2;
+				if (numBodies>0)
+					m_analyticsData.m_islandId = bodies[0]->getCompanionId();
+				m_analyticsData.m_numBodies = numBodies;
+				m_analyticsData.m_numContactManifolds = numManifolds;
+				m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
+				break;
+			}
+		}
 	}
 	return 0.f;
 }
 
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
+void btSequentialImpulseConstraintSolver::writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
 {
-	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-	int i,j;
-
-	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	for (int j = iBegin; j < iEnd; j++)
 	{
-		for (j=0;j<numPoolConstraints;j++)
-		{
-			const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
-			btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
-			btAssert(pt);
-			pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
+		const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
+		btManifoldPoint* pt = (btManifoldPoint*)solveManifold.m_originalContactPoint;
+		btAssert(pt);
+		pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
 		//	float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-			//	printf("pt->m_appliedImpulseLateral1 = %f\n", f);
-			pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-			//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
-			if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-			{
-				pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
-			}
-			//do a callback here?
+		//	printf("pt->m_appliedImpulseLateral1 = %f\n", f);
+		pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+		//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
+		if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+		{
+			pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse;
 		}
+		//do a callback here?
 	}
+}
 
-	numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
-	for (j=0;j<numPoolConstraints;j++)
+void btSequentialImpulseConstraintSolver::writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
+	for (int j = iBegin; j < iEnd; j++)
 	{
 		const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
 		btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
 		btJointFeedback* fb = constr->getJointFeedback();
 		if (fb)
 		{
-			fb->m_appliedForceBodyA += solverConstr.m_contactNormal1*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
-			fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
-			fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
-			fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
-
+			fb->m_appliedForceBodyA += solverConstr.m_contactNormal1 * solverConstr.m_appliedImpulse * constr->getRigidBodyA().getLinearFactor() / infoGlobal.m_timeStep;
+			fb->m_appliedForceBodyB += solverConstr.m_contactNormal2 * solverConstr.m_appliedImpulse * constr->getRigidBodyB().getLinearFactor() / infoGlobal.m_timeStep;
+			fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal * constr->getRigidBodyA().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep;
+			fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal * constr->getRigidBodyB().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; /*RGM ???? */
 		}
 
 		constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
-		if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
+		if (btFabs(solverConstr.m_appliedImpulse) >= constr->getBreakingImpulseThreshold())
 		{
 			constr->setEnabled(false);
 		}
 	}
+}
 
-
-
-	for ( i=0;i<m_tmpSolverBodyPool.size();i++)
+void btSequentialImpulseConstraintSolver::writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
+	for (int i = iBegin; i < iEnd; i++)
 	{
 		btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
 		if (body)
@@ -1882,11 +1818,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
 				m_tmpSolverBodyPool[i].writebackVelocity();
 
 			m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
-				m_tmpSolverBodyPool[i].m_linearVelocity+
+				m_tmpSolverBodyPool[i].m_linearVelocity +
 				m_tmpSolverBodyPool[i].m_externalForceImpulse);
 
 			m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
-				m_tmpSolverBodyPool[i].m_angularVelocity+
+				m_tmpSolverBodyPool[i].m_angularVelocity +
 				m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
 
 			if (infoGlobal.m_splitImpulse)
@@ -1895,6 +1831,19 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
 			m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
 		}
 	}
+}
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("solveGroupCacheFriendlyFinish");
+
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	{
+		writeBackContacts(0, m_tmpSolverContactConstraintPool.size(), infoGlobal);
+	}
+
+	writeBackJoints(0, m_tmpSolverNonContactConstraintPool.size(), infoGlobal);
+	writeBackBodies(0, m_tmpSolverBodyPool.size(), infoGlobal);
 
 	m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
 	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
@@ -1905,25 +1854,22 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
 	return 0.f;
 }
 
-
-
 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
-btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btDispatcher* /*dispatcher*/)
+btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer, btDispatcher* /*dispatcher*/)
 {
-
 	BT_PROFILE("solveGroup");
 	//you need to provide at least some bodies
 
-	solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
+	solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
-	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
+	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
 	solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
 
 	return 0.f;
 }
 
-void	btSequentialImpulseConstraintSolver::reset()
+void btSequentialImpulseConstraintSolver::reset()
 {
 	m_btSeed2 = 0;
 }
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
index a6029180983..f3ef02fccc7 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
@@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -27,121 +27,161 @@ class btCollisionObject;
 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
 #include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
 
-typedef btSimdScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
+typedef btScalar (*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
+
+struct btSolverAnalyticsData
+{
+	btSolverAnalyticsData()
+	{
+		m_numSolverCalls = 0;
+		m_numIterationsUsed = -1;
+		m_remainingLeastSquaresResidual = -1;
+		m_islandId = -2;
+	}
+	int m_islandId;
+	int m_numBodies;
+	int m_numContactManifolds;
+	int m_numSolverCalls;
+	int m_numIterationsUsed;
+	double m_remainingLeastSquaresResidual;
+};
 
 ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
-ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver
+ATTRIBUTE_ALIGNED16(class)
+btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
+	
+
 protected:
-	btAlignedObjectArray<btSolverBody>      m_tmpSolverBodyPool;
-	btConstraintArray			m_tmpSolverContactConstraintPool;
-	btConstraintArray			m_tmpSolverNonContactConstraintPool;
-	btConstraintArray			m_tmpSolverContactFrictionConstraintPool;
-	btConstraintArray			m_tmpSolverContactRollingFrictionConstraintPool;
-
-	btAlignedObjectArray<int>	m_orderTmpConstraintPool;
-	btAlignedObjectArray<int>	m_orderNonContactConstraintPool;
-	btAlignedObjectArray<int>	m_orderFrictionConstraintPool;
+	btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
+	btConstraintArray m_tmpSolverContactConstraintPool;
+	btConstraintArray m_tmpSolverNonContactConstraintPool;
+	btConstraintArray m_tmpSolverContactFrictionConstraintPool;
+	btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
+
+	btAlignedObjectArray<int> m_orderTmpConstraintPool;
+	btAlignedObjectArray<int> m_orderNonContactConstraintPool;
+	btAlignedObjectArray<int> m_orderFrictionConstraintPool;
 	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
-	int							m_maxOverrideNumSolverIterations;
+	int m_maxOverrideNumSolverIterations;
 	int m_fixedBodyId;
+	// When running solvers on multiple threads, a race condition exists for Kinematic objects that
+	// participate in more than one solver.
+	// The getOrInitSolverBody() function writes the companionId of each body (storing the index of the solver body
+	// for the current solver). For normal dynamic bodies it isn't an issue because they can only be in one island
+	// (and therefore one thread) at a time. But kinematic bodies can be in multiple islands at once.
+	// To avoid this race condition, this solver does not write the companionId, instead it stores the solver body
+	// index in this solver-local table, indexed by the uniqueId of the body.
+	btAlignedObjectArray<int> m_kinematicBodyUniqueIdToSolverBodyTable;  // only used for multithreading
 
 	btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
 	btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
+	btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse;
+	int m_cachedSolverMode;  // used to check if SOLVER_SIMD flag has been changed
+	void setupSolverFunctions(bool useSimd);
 
-	void setupFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
-									btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
-									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
-									btScalar desiredVelocity=0., btScalar cfmSlip=0.);
+	btScalar m_leastSquaresResidual;
 
-	void setupRollingFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
-									btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
-									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
-									btScalar desiredVelocity=0., btScalar cfmSlip=0.);
+	void setupFrictionConstraint(btSolverConstraint & solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
+		btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2,
+		btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
+		const btContactSolverInfo& infoGlobal,
+		btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
 
-	btSolverConstraint&	addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
-	btSolverConstraint&	addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
+	void setupTorsionalFrictionConstraint(btSolverConstraint & solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
+		btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
+		btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
+		btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
 
-	
-	void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, 
-								const btContactSolverInfo& infoGlobal,btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
+	btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
+	btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar torsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity = 0, btScalar cfmSlip = 0.f);
+
+	void setupContactConstraint(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
+		const btContactSolverInfo& infoGlobal, btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
 
-	static void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
+	static void applyAnisotropicFriction(btCollisionObject * colObj, btVector3 & frictionDirection, int frictionMode);
 
-	void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, 
-										 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
+	void setFrictionConstraintImpulse(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB,
+		btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
 
 	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
-	unsigned long	m_btSeed2;
+	unsigned long m_btSeed2;
 
-	
-	btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
+	btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold);
+
+	virtual void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
 
-	virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
+	void convertContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal);
 
-	void	convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
+	virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
+	void convertJoint(btSolverConstraint * currentConstraintRow, btTypedConstraint * constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal);
 
+	virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
 
-	void	resolveSplitPenetrationSIMD(
-     btSolverBody& bodyA,btSolverBody& bodyB,
-        const btSolverConstraint& contactConstraint);
+	btScalar resolveSplitPenetrationSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
+	{
+		return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
+	}
 
-	void	resolveSplitPenetrationImpulseCacheFriendly(
-       btSolverBody& bodyA,btSolverBody& bodyB,
-        const btSolverConstraint& contactConstraint);
+	btScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
+	{
+		return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
+	}
 
 	//internal method
-	int		getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
-	void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);
-
-	btSimdScalar	resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-	btSimdScalar	resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-	btSimdScalar	resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-	btSimdScalar	resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-		
-protected:
-	
-	
-	virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
-	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+	int getOrInitSolverBody(btCollisionObject & body, btScalar timeStep);
+	void initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep);
+
+	btScalar resolveSingleConstraintRowGeneric(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
+	btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
+	btScalar resolveSingleConstraintRowLowerLimit(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
+	btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
+	btScalar resolveSplitPenetrationImpulse(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
+	{
+		return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
+	}
 
-	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+protected:
+	void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+	void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+	void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+	virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
+	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 
+	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
+
 	btSequentialImpulseConstraintSolver();
 	virtual ~btSequentialImpulseConstraintSolver();
 
-	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
-		
+	virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
+
 	///clear internal cached data and reset random seed
-	virtual	void	reset();
-	
+	virtual void reset();
+
 	unsigned long btRand2();
 
-	int btRandInt2 (int n);
+	int btRandInt2(int n);
 
-	void	setRandSeed(unsigned long seed)
+	void setRandSeed(unsigned long seed)
 	{
 		m_btSeed2 = seed;
 	}
-	unsigned long	getRandSeed() const
+	unsigned long getRandSeed() const
 	{
 		return m_btSeed2;
 	}
 
-	
-	virtual btConstraintSolverType	getSolverType() const
+	virtual btConstraintSolverType getSolverType() const
 	{
 		return BT_SEQUENTIAL_IMPULSE_SOLVER;
 	}
 
-	btSingleConstraintRowSolver	getActiveConstraintRowSolverGeneric()
+	btSingleConstraintRowSolver getActiveConstraintRowSolverGeneric()
 	{
 		return m_resolveSingleConstraintRowGeneric;
 	}
@@ -149,7 +189,7 @@ public:
 	{
 		m_resolveSingleConstraintRowGeneric = rowSolver;
 	}
-	btSingleConstraintRowSolver	getActiveConstraintRowSolverLowerLimit()
+	btSingleConstraintRowSolver getActiveConstraintRowSolverLowerLimit()
 	{
 		return m_resolveSingleConstraintRowLowerLimit;
 	}
@@ -158,19 +198,18 @@ public:
 		m_resolveSingleConstraintRowLowerLimit = rowSolver;
 	}
 
+
+
 	///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4
-	btSingleConstraintRowSolver	getScalarConstraintRowSolverGeneric();
-	btSingleConstraintRowSolver	getSSE2ConstraintRowSolverGeneric();
-	btSingleConstraintRowSolver	getSSE4_1ConstraintRowSolverGeneric();
+	btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
+	btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
+	btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
 
 	///Various implementations of solving a single constraint row using an inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4
-	btSingleConstraintRowSolver	getScalarConstraintRowSolverLowerLimit();
-	btSingleConstraintRowSolver	getSSE2ConstraintRowSolverLowerLimit();
-	btSingleConstraintRowSolver	getSSE4_1ConstraintRowSolverLowerLimit();
+	btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
+	btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
+	btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
+	btSolverAnalyticsData m_analyticsData;
 };
 
-
-
-
-#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
-
+#endif  //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp
new file mode 100644
index 00000000000..2718da4a501
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp
@@ -0,0 +1,1554 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btSequentialImpulseConstraintSolverMt.h"
+
+#include "LinearMath/btQuickprof.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+
+bool btSequentialImpulseConstraintSolverMt::s_allowNestedParallelForLoops = false;  // some task schedulers don't like nested loops
+int btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching = 250;
+int btSequentialImpulseConstraintSolverMt::s_minBatchSize = 50;
+int btSequentialImpulseConstraintSolverMt::s_maxBatchSize = 100;
+btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_contactBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D;
+btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_jointBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D;
+
+btSequentialImpulseConstraintSolverMt::btSequentialImpulseConstraintSolverMt()
+{
+	m_numFrictionDirections = 1;
+	m_useBatching = false;
+	m_useObsoleteJointConstraints = false;
+}
+
+btSequentialImpulseConstraintSolverMt::~btSequentialImpulseConstraintSolverMt()
+{
+}
+
+void btSequentialImpulseConstraintSolverMt::setupBatchedContactConstraints()
+{
+	BT_PROFILE("setupBatchedContactConstraints");
+	m_batchedContactConstraints.setup(&m_tmpSolverContactConstraintPool,
+									  m_tmpSolverBodyPool,
+									  s_contactBatchingMethod,
+									  s_minBatchSize,
+									  s_maxBatchSize,
+									  &m_scratchMemory);
+}
+
+void btSequentialImpulseConstraintSolverMt::setupBatchedJointConstraints()
+{
+	BT_PROFILE("setupBatchedJointConstraints");
+	m_batchedJointConstraints.setup(&m_tmpSolverNonContactConstraintPool,
+									m_tmpSolverBodyPool,
+									s_jointBatchingMethod,
+									s_minBatchSize,
+									s_maxBatchSize,
+									&m_scratchMemory);
+}
+
+void btSequentialImpulseConstraintSolverMt::internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal)
+{
+	btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[iContactConstraint];
+
+	btVector3 rel_pos1;
+	btVector3 rel_pos2;
+	btScalar relaxation;
+
+	int solverBodyIdA = contactConstraint.m_solverBodyIdA;
+	int solverBodyIdB = contactConstraint.m_solverBodyIdB;
+
+	btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
+	btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
+
+	btRigidBody* colObj0 = solverBodyA->m_originalBody;
+	btRigidBody* colObj1 = solverBodyB->m_originalBody;
+
+	btManifoldPoint& cp = *static_cast<btManifoldPoint*>(contactConstraint.m_originalContactPoint);
+
+	const btVector3& pos1 = cp.getPositionWorldOnA();
+	const btVector3& pos2 = cp.getPositionWorldOnB();
+
+	rel_pos1 = pos1 - solverBodyA->getWorldTransform().getOrigin();
+	rel_pos2 = pos2 - solverBodyB->getWorldTransform().getOrigin();
+
+	btVector3 vel1;
+	btVector3 vel2;
+
+	solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1, vel1);
+	solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2, vel2);
+
+	btVector3 vel = vel1 - vel2;
+	btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
+
+	setupContactConstraint(contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
+
+	// setup rolling friction constraints
+	int rollingFrictionIndex = m_rollingFrictionIndexTable[iContactConstraint];
+	if (rollingFrictionIndex >= 0)
+	{
+		btSolverConstraint& spinningFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[rollingFrictionIndex];
+		btAssert(spinningFrictionConstraint.m_frictionIndex == iContactConstraint);
+		setupTorsionalFrictionConstraint(spinningFrictionConstraint,
+										 cp.m_normalWorldOnB,
+										 solverBodyIdA,
+										 solverBodyIdB,
+										 cp,
+										 cp.m_combinedSpinningFriction,
+										 rel_pos1,
+										 rel_pos2,
+										 colObj0,
+										 colObj1,
+										 relaxation,
+										 0.0f,
+										 0.0f);
+		btVector3 axis[2];
+		btPlaneSpace1(cp.m_normalWorldOnB, axis[0], axis[1]);
+		axis[0].normalize();
+		axis[1].normalize();
+
+		applyAnisotropicFriction(colObj0, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+		applyAnisotropicFriction(colObj1, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+		applyAnisotropicFriction(colObj0, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+		applyAnisotropicFriction(colObj1, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+		// put the largest axis first
+		if (axis[1].length2() > axis[0].length2())
+		{
+			btSwap(axis[0], axis[1]);
+		}
+		const btScalar kRollingFrictionThreshold = 0.001f;
+		for (int i = 0; i < 2; ++i)
+		{
+			int iRollingFric = rollingFrictionIndex + 1 + i;
+			btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric];
+			btAssert(rollingFrictionConstraint.m_frictionIndex == iContactConstraint);
+			btVector3 dir = axis[i];
+			if (dir.length() > kRollingFrictionThreshold)
+			{
+				setupTorsionalFrictionConstraint(rollingFrictionConstraint,
+												 dir,
+												 solverBodyIdA,
+												 solverBodyIdB,
+												 cp,
+												 cp.m_combinedRollingFriction,
+												 rel_pos1,
+												 rel_pos2,
+												 colObj0,
+												 colObj1,
+												 relaxation,
+												 0.0f,
+												 0.0f);
+			}
+			else
+			{
+				rollingFrictionConstraint.m_frictionIndex = -1;  // disable constraint
+			}
+		}
+	}
+
+	// setup friction constraints
+	//	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
+	{
+		///Bullet has several options to set the friction directions
+		///By default, each contact has only a single friction direction that is recomputed automatically very frame
+		///based on the relative linear velocity.
+		///If the relative velocity it zero, it will automatically compute a friction direction.
+
+		///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
+		///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
+		///
+		///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
+		///
+		///The user can manually override the friction directions for certain contacts using a contact callback,
+		///and set the cp.m_lateralFrictionInitialized to true
+		///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
+		///this will give a conveyor belt effect
+		///
+		btSolverConstraint* frictionConstraint1 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex];
+		btAssert(frictionConstraint1->m_frictionIndex == iContactConstraint);
+
+		btSolverConstraint* frictionConstraint2 = NULL;
+		if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
+		{
+			frictionConstraint2 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex + 1];
+			btAssert(frictionConstraint2->m_frictionIndex == iContactConstraint);
+		}
+
+		if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
+		{
+			cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+			btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+			if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
+			{
+				cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel);
+				applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+				applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+				setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
+
+				if (frictionConstraint2)
+				{
+					cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
+					cp.m_lateralFrictionDir2.normalize();  //??
+					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
+				}
+			}
+			else
+			{
+				btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2);
+
+				applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+				applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+				setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
+
+				if (frictionConstraint2)
+				{
+					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
+				}
+
+				if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
+				{
+					cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
+				}
+			}
+		}
+		else
+		{
+			setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
+			if (frictionConstraint2)
+			{
+				setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
+			}
+		}
+	}
+
+	setFrictionConstraintImpulse(contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
+}
+
+struct SetupContactConstraintsLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
+	const btContactSolverInfo* m_infoGlobal;
+
+	SetupContactConstraintsLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, const btContactSolverInfo& infoGlobal)
+	{
+		m_solver = solver;
+		m_bc = bc;
+		m_infoGlobal = &infoGlobal;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("SetupContactConstraintsLoop");
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			for (int i = batch.begin; i < batch.end; ++i)
+			{
+				int iContact = m_bc->m_constraintIndices[i];
+				m_solver->internalSetupContactConstraints(iContact, *m_infoGlobal);
+			}
+		}
+	}
+};
+
+void btSequentialImpulseConstraintSolverMt::setupAllContactConstraints(const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("setupAllContactConstraints");
+	if (m_useBatching)
+	{
+		const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+		SetupContactConstraintsLoop loop(this, &batchedCons, infoGlobal);
+		for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+		{
+			int iPhase = batchedCons.m_phaseOrder[iiPhase];
+			const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+			int grainSize = 1;
+			btParallelFor(phase.begin, phase.end, grainSize, loop);
+		}
+	}
+	else
+	{
+		for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); ++i)
+		{
+			internalSetupContactConstraints(i, infoGlobal);
+		}
+	}
+}
+
+int btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btCollisionObject& body, btScalar timeStep)
+{
+	//
+	// getOrInitSolverBody is threadsafe only for a single thread per solver (with potentially multiple solvers)
+	//
+	// getOrInitSolverBodyThreadsafe -- attempts to be fully threadsafe (however may affect determinism)
+	//
+	int solverBodyId = -1;
+	bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
+	if (isRigidBodyType && !body.isStaticOrKinematicObject())
+	{
+		// dynamic body
+		// Dynamic bodies can only be in one island, so it's safe to write to the companionId
+		solverBodyId = body.getCompanionId();
+		if (solverBodyId < 0)
+		{
+			m_bodySolverArrayMutex.lock();
+			// now that we have the lock, check again
+			solverBodyId = body.getCompanionId();
+			if (solverBodyId < 0)
+			{
+				solverBodyId = m_tmpSolverBodyPool.size();
+				btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+				initSolverBody(&solverBody, &body, timeStep);
+				body.setCompanionId(solverBodyId);
+			}
+			m_bodySolverArrayMutex.unlock();
+		}
+	}
+	else if (isRigidBodyType && body.isKinematicObject())
+	{
+		//
+		// NOTE: must test for kinematic before static because some kinematic objects also
+		//   identify as "static"
+		//
+		// Kinematic bodies can be in multiple islands at once, so it is a
+		// race condition to write to them, so we use an alternate method
+		// to record the solverBodyId
+		int uniqueId = body.getWorldArrayIndex();
+		const int INVALID_SOLVER_BODY_ID = -1;
+		if (m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId)
+		{
+			m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock();
+			// now that we have the lock, check again
+			if (m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId)
+			{
+				m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
+			}
+			m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock();
+		}
+		solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId];
+		// if no table entry yet,
+		if (INVALID_SOLVER_BODY_ID == solverBodyId)
+		{
+			// need to acquire both locks
+			m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock();
+			m_bodySolverArrayMutex.lock();
+			// now that we have the lock, check again
+			solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId];
+			if (INVALID_SOLVER_BODY_ID == solverBodyId)
+			{
+				// create a table entry for this body
+				solverBodyId = m_tmpSolverBodyPool.size();
+				btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+				initSolverBody(&solverBody, &body, timeStep);
+				m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId] = solverBodyId;
+			}
+			m_bodySolverArrayMutex.unlock();
+			m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock();
+		}
+	}
+	else
+	{
+		// all fixed bodies (inf mass) get mapped to a single solver id
+		if (m_fixedBodyId < 0)
+		{
+			m_bodySolverArrayMutex.lock();
+			// now that we have the lock, check again
+			if (m_fixedBodyId < 0)
+			{
+				m_fixedBodyId = m_tmpSolverBodyPool.size();
+				btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+				initSolverBody(&fixedBody, 0, timeStep);
+			}
+			m_bodySolverArrayMutex.unlock();
+		}
+		solverBodyId = m_fixedBodyId;
+	}
+	btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size());
+	return solverBodyId;
+}
+
+void btSequentialImpulseConstraintSolverMt::internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("internalCollectContactManifoldCachedInfo");
+	for (int i = 0; i < numManifolds; ++i)
+	{
+		btContactManifoldCachedInfo* cachedInfo = &cachedInfoArray[i];
+		btPersistentManifold* manifold = manifoldPtr[i];
+		btCollisionObject* colObj0 = (btCollisionObject*)manifold->getBody0();
+		btCollisionObject* colObj1 = (btCollisionObject*)manifold->getBody1();
+
+		int solverBodyIdA = getOrInitSolverBodyThreadsafe(*colObj0, infoGlobal.m_timeStep);
+		int solverBodyIdB = getOrInitSolverBodyThreadsafe(*colObj1, infoGlobal.m_timeStep);
+
+		cachedInfo->solverBodyIds[0] = solverBodyIdA;
+		cachedInfo->solverBodyIds[1] = solverBodyIdB;
+		cachedInfo->numTouchingContacts = 0;
+
+		btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
+		btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
+
+		// A contact manifold between 2 static object should not exist!
+		// check the collision flags of your objects if this assert fires.
+		// Incorrectly set collision object flags can degrade performance in various ways.
+		btAssert(!m_tmpSolverBodyPool[solverBodyIdA].m_invMass.isZero() || !m_tmpSolverBodyPool[solverBodyIdB].m_invMass.isZero());
+
+		int iContact = 0;
+		for (int j = 0; j < manifold->getNumContacts(); j++)
+		{
+			btManifoldPoint& cp = manifold->getContactPoint(j);
+
+			if (cp.getDistance() <= manifold->getContactProcessingThreshold())
+			{
+				cachedInfo->contactPoints[iContact] = &cp;
+				cachedInfo->contactHasRollingFriction[iContact] = (cp.m_combinedRollingFriction > 0.f);
+				iContact++;
+			}
+		}
+		cachedInfo->numTouchingContacts = iContact;
+	}
+}
+
+struct CollectContactManifoldCachedInfoLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray;
+	btPersistentManifold** m_manifoldPtr;
+	const btContactSolverInfo* m_infoGlobal;
+
+	CollectContactManifoldCachedInfoLoop(btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, const btContactSolverInfo& infoGlobal)
+	{
+		m_solver = solver;
+		m_cachedInfoArray = cachedInfoArray;
+		m_manifoldPtr = manifoldPtr;
+		m_infoGlobal = &infoGlobal;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalCollectContactManifoldCachedInfo(m_cachedInfoArray + iBegin, m_manifoldPtr + iBegin, iEnd - iBegin, *m_infoGlobal);
+	}
+};
+
+void btSequentialImpulseConstraintSolverMt::internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds)
+{
+	BT_PROFILE("internalAllocContactConstraints");
+	// possibly parallel part
+	for (int iManifold = 0; iManifold < numManifolds; ++iManifold)
+	{
+		const btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[iManifold];
+		int contactIndex = cachedInfo.contactIndex;
+		int frictionIndex = contactIndex * m_numFrictionDirections;
+		int rollingFrictionIndex = cachedInfo.rollingFrictionIndex;
+		for (int i = 0; i < cachedInfo.numTouchingContacts; i++)
+		{
+			btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[contactIndex];
+			contactConstraint.m_solverBodyIdA = cachedInfo.solverBodyIds[0];
+			contactConstraint.m_solverBodyIdB = cachedInfo.solverBodyIds[1];
+			contactConstraint.m_originalContactPoint = cachedInfo.contactPoints[i];
+
+			// allocate the friction constraints
+			contactConstraint.m_frictionIndex = frictionIndex;
+			for (int iDir = 0; iDir < m_numFrictionDirections; ++iDir)
+			{
+				btSolverConstraint& frictionConstraint = m_tmpSolverContactFrictionConstraintPool[frictionIndex];
+				frictionConstraint.m_frictionIndex = contactIndex;
+				frictionIndex++;
+			}
+
+			// allocate rolling friction constraints
+			if (cachedInfo.contactHasRollingFriction[i])
+			{
+				m_rollingFrictionIndexTable[contactIndex] = rollingFrictionIndex;
+				// allocate 3 (although we may use only 2 sometimes)
+				for (int i = 0; i < 3; i++)
+				{
+					m_tmpSolverContactRollingFrictionConstraintPool[rollingFrictionIndex].m_frictionIndex = contactIndex;
+					rollingFrictionIndex++;
+				}
+			}
+			else
+			{
+				// indicate there is no rolling friction for this contact point
+				m_rollingFrictionIndexTable[contactIndex] = -1;
+			}
+			contactIndex++;
+		}
+	}
+}
+
+struct AllocContactConstraintsLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray;
+
+	AllocContactConstraintsLoop(btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray)
+	{
+		m_solver = solver;
+		m_cachedInfoArray = cachedInfoArray;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalAllocContactConstraints(m_cachedInfoArray + iBegin, iEnd - iBegin);
+	}
+};
+
+void btSequentialImpulseConstraintSolverMt::allocAllContactConstraints(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("allocAllContactConstraints");
+	btAlignedObjectArray<btContactManifoldCachedInfo> cachedInfoArray;  // = m_manifoldCachedInfoArray;
+	cachedInfoArray.resizeNoInitialize(numManifolds);
+	if (/* DISABLES CODE */ (false))
+	{
+		// sequential
+		internalCollectContactManifoldCachedInfo(&cachedInfoArray[0], manifoldPtr, numManifolds, infoGlobal);
+	}
+	else
+	{
+		// may alter ordering of bodies which affects determinism
+		CollectContactManifoldCachedInfoLoop loop(this, &cachedInfoArray[0], manifoldPtr, infoGlobal);
+		int grainSize = 200;
+		btParallelFor(0, numManifolds, grainSize, loop);
+	}
+
+	{
+		// serial part
+		int numContacts = 0;
+		int numRollingFrictionConstraints = 0;
+		for (int iManifold = 0; iManifold < numManifolds; ++iManifold)
+		{
+			btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[iManifold];
+			cachedInfo.contactIndex = numContacts;
+			cachedInfo.rollingFrictionIndex = numRollingFrictionConstraints;
+			numContacts += cachedInfo.numTouchingContacts;
+			for (int i = 0; i < cachedInfo.numTouchingContacts; ++i)
+			{
+				if (cachedInfo.contactHasRollingFriction[i])
+				{
+					numRollingFrictionConstraints += 3;
+				}
+			}
+		}
+		{
+			BT_PROFILE("allocPools");
+			if (m_tmpSolverContactConstraintPool.capacity() < numContacts)
+			{
+				// if we need to reallocate, reserve some extra so we don't have to reallocate again next frame
+				int extraReserve = numContacts / 16;
+				m_tmpSolverContactConstraintPool.reserve(numContacts + extraReserve);
+				m_rollingFrictionIndexTable.reserve(numContacts + extraReserve);
+				m_tmpSolverContactFrictionConstraintPool.reserve((numContacts + extraReserve) * m_numFrictionDirections);
+				m_tmpSolverContactRollingFrictionConstraintPool.reserve(numRollingFrictionConstraints + extraReserve);
+			}
+			m_tmpSolverContactConstraintPool.resizeNoInitialize(numContacts);
+			m_rollingFrictionIndexTable.resizeNoInitialize(numContacts);
+			m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(numContacts * m_numFrictionDirections);
+			m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(numRollingFrictionConstraints);
+		}
+	}
+	{
+		AllocContactConstraintsLoop loop(this, &cachedInfoArray[0]);
+		int grainSize = 200;
+		btParallelFor(0, numManifolds, grainSize, loop);
+	}
+}
+
+void btSequentialImpulseConstraintSolverMt::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
+{
+	if (!m_useBatching)
+	{
+		btSequentialImpulseConstraintSolver::convertContacts(manifoldPtr, numManifolds, infoGlobal);
+		return;
+	}
+	BT_PROFILE("convertContacts");
+	if (numManifolds > 0)
+	{
+		if (m_fixedBodyId < 0)
+		{
+			m_fixedBodyId = m_tmpSolverBodyPool.size();
+			btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+			initSolverBody(&fixedBody, 0, infoGlobal.m_timeStep);
+		}
+		allocAllContactConstraints(manifoldPtr, numManifolds, infoGlobal);
+		if (m_useBatching)
+		{
+			setupBatchedContactConstraints();
+		}
+		setupAllContactConstraints(infoGlobal);
+	}
+}
+
+void btSequentialImpulseConstraintSolverMt::internalInitMultipleJoints(btTypedConstraint** constraints, int iBegin, int iEnd)
+{
+	BT_PROFILE("internalInitMultipleJoints");
+	for (int i = iBegin; i < iEnd; i++)
+	{
+		btTypedConstraint* constraint = constraints[i];
+		btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+		if (constraint->isEnabled())
+		{
+			constraint->buildJacobian();
+			constraint->internalSetAppliedImpulse(0.0f);
+			btJointFeedback* fb = constraint->getJointFeedback();
+			if (fb)
+			{
+				fb->m_appliedForceBodyA.setZero();
+				fb->m_appliedTorqueBodyA.setZero();
+				fb->m_appliedForceBodyB.setZero();
+				fb->m_appliedTorqueBodyB.setZero();
+			}
+			constraint->getInfo1(&info1);
+		}
+		else
+		{
+			info1.m_numConstraintRows = 0;
+			info1.nub = 0;
+		}
+	}
+}
+
+struct InitJointsLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	btTypedConstraint** m_constraints;
+
+	InitJointsLoop(btSequentialImpulseConstraintSolverMt* solver, btTypedConstraint** constraints)
+	{
+		m_solver = solver;
+		m_constraints = constraints;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalInitMultipleJoints(m_constraints, iBegin, iEnd);
+	}
+};
+
+void btSequentialImpulseConstraintSolverMt::internalConvertMultipleJoints(const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("internalConvertMultipleJoints");
+	for (int i = iBegin; i < iEnd; ++i)
+	{
+		const JointParams& jointParams = jointParamsArray[i];
+		int currentRow = jointParams.m_solverConstraint;
+		if (currentRow != -1)
+		{
+			const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+			btAssert(currentRow < m_tmpSolverNonContactConstraintPool.size());
+			btAssert(info1.m_numConstraintRows > 0);
+
+			btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
+			btTypedConstraint* constraint = constraints[i];
+
+			convertJoint(currentConstraintRow, constraint, info1, jointParams.m_solverBodyA, jointParams.m_solverBodyB, infoGlobal);
+		}
+	}
+}
+
+struct ConvertJointsLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& m_jointParamsArray;
+	btTypedConstraint** m_srcConstraints;
+	const btContactSolverInfo& m_infoGlobal;
+
+	ConvertJointsLoop(btSequentialImpulseConstraintSolverMt* solver,
+					  const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& jointParamsArray,
+					  btTypedConstraint** srcConstraints,
+					  const btContactSolverInfo& infoGlobal) : m_jointParamsArray(jointParamsArray),
+															   m_infoGlobal(infoGlobal)
+	{
+		m_solver = solver;
+		m_srcConstraints = srcConstraints;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalConvertMultipleJoints(m_jointParamsArray, m_srcConstraints, iBegin, iEnd, m_infoGlobal);
+	}
+};
+
+void btSequentialImpulseConstraintSolverMt::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal)
+{
+	if (!m_useBatching)
+	{
+		btSequentialImpulseConstraintSolver::convertJoints(constraints, numConstraints, infoGlobal);
+		return;
+	}
+	BT_PROFILE("convertJoints");
+	bool parallelJointSetup = true;
+	m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
+	if (parallelJointSetup)
+	{
+		InitJointsLoop loop(this, constraints);
+		int grainSize = 40;
+		btParallelFor(0, numConstraints, grainSize, loop);
+	}
+	else
+	{
+		internalInitMultipleJoints(constraints, 0, numConstraints);
+	}
+
+	int totalNumRows = 0;
+	btAlignedObjectArray<JointParams> jointParamsArray;
+	jointParamsArray.resizeNoInitialize(numConstraints);
+
+	//calculate the total number of contraint rows
+	for (int i = 0; i < numConstraints; i++)
+	{
+		btTypedConstraint* constraint = constraints[i];
+
+		JointParams& params = jointParamsArray[i];
+		const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+
+		if (info1.m_numConstraintRows)
+		{
+			params.m_solverConstraint = totalNumRows;
+			params.m_solverBodyA = getOrInitSolverBody(constraint->getRigidBodyA(), infoGlobal.m_timeStep);
+			params.m_solverBodyB = getOrInitSolverBody(constraint->getRigidBodyB(), infoGlobal.m_timeStep);
+		}
+		else
+		{
+			params.m_solverConstraint = -1;
+		}
+		totalNumRows += info1.m_numConstraintRows;
+	}
+	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
+
+	///setup the btSolverConstraints
+	if (parallelJointSetup)
+	{
+		ConvertJointsLoop loop(this, jointParamsArray, constraints, infoGlobal);
+		int grainSize = 20;
+		btParallelFor(0, numConstraints, grainSize, loop);
+	}
+	else
+	{
+		internalConvertMultipleJoints(jointParamsArray, constraints, 0, numConstraints, infoGlobal);
+	}
+	setupBatchedJointConstraints();
+}
+
+void btSequentialImpulseConstraintSolverMt::internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("internalConvertBodies");
+	for (int i = iBegin; i < iEnd; i++)
+	{
+		btCollisionObject* obj = bodies[i];
+		obj->setCompanionId(i);
+		btSolverBody& solverBody = m_tmpSolverBodyPool[i];
+		initSolverBody(&solverBody, obj, infoGlobal.m_timeStep);
+
+		btRigidBody* body = btRigidBody::upcast(obj);
+		if (body && body->getInvMass())
+		{
+			btVector3 gyroForce(0, 0, 0);
+			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
+			{
+				gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
+				solverBody.m_externalTorqueImpulse -= gyroForce * body->getInvInertiaTensorWorld() * infoGlobal.m_timeStep;
+			}
+			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
+			{
+				gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
+				solverBody.m_externalTorqueImpulse += gyroForce;
+			}
+			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
+			{
+				gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
+				solverBody.m_externalTorqueImpulse += gyroForce;
+			}
+		}
+	}
+}
+
+struct ConvertBodiesLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	btCollisionObject** m_bodies;
+	int m_numBodies;
+	const btContactSolverInfo& m_infoGlobal;
+
+	ConvertBodiesLoop(btSequentialImpulseConstraintSolverMt* solver,
+					  btCollisionObject** bodies,
+					  int numBodies,
+					  const btContactSolverInfo& infoGlobal) : m_infoGlobal(infoGlobal)
+	{
+		m_solver = solver;
+		m_bodies = bodies;
+		m_numBodies = numBodies;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalConvertBodies(m_bodies, iBegin, iEnd, m_infoGlobal);
+	}
+};
+
+void btSequentialImpulseConstraintSolverMt::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("convertBodies");
+	m_kinematicBodyUniqueIdToSolverBodyTable.resize(0);
+
+	m_tmpSolverBodyPool.resizeNoInitialize(numBodies + 1);
+
+	m_fixedBodyId = numBodies;
+	{
+		btSolverBody& fixedBody = m_tmpSolverBodyPool[m_fixedBodyId];
+		initSolverBody(&fixedBody, NULL, infoGlobal.m_timeStep);
+	}
+
+	bool parallelBodySetup = true;
+	if (parallelBodySetup)
+	{
+		ConvertBodiesLoop loop(this, bodies, numBodies, infoGlobal);
+		int grainSize = 40;
+		btParallelFor(0, numBodies, grainSize, loop);
+	}
+	else
+	{
+		internalConvertBodies(bodies, 0, numBodies, infoGlobal);
+	}
+}
+
+btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySetup(
+	btCollisionObject** bodies,
+	int numBodies,
+	btPersistentManifold** manifoldPtr,
+	int numManifolds,
+	btTypedConstraint** constraints,
+	int numConstraints,
+	const btContactSolverInfo& infoGlobal,
+	btIDebugDraw* debugDrawer)
+{
+	m_numFrictionDirections = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
+	m_useBatching = false;
+	if (numManifolds >= s_minimumContactManifoldsForBatching &&
+		(s_allowNestedParallelForLoops || !btThreadsAreRunning()))
+	{
+		m_useBatching = true;
+		m_batchedContactConstraints.m_debugDrawer = debugDrawer;
+		m_batchedJointConstraints.m_debugDrawer = debugDrawer;
+	}
+	btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies,
+																	  numBodies,
+																	  manifoldPtr,
+																	  numManifolds,
+																	  constraints,
+																	  numConstraints,
+																	  infoGlobal,
+																	  debugDrawer);
+	return 0.0f;
+}
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactSplitPenetrationImpulseConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
+{
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
+	{
+		int iCons = consIndices[iiCons];
+		const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iCons];
+		btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
+		btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
+		btScalar residual = resolveSplitPenetrationImpulse(bodyA, bodyB, solveManifold);
+		leastSquaresResidual += residual * residual;
+	}
+	return leastSquaresResidual;
+}
+
+struct ContactSplitPenetrationImpulseSolverLoop : public btIParallelSumBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
+
+	ContactSplitPenetrationImpulseSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
+	{
+		m_solver = solver;
+		m_bc = bc;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("ContactSplitPenetrationImpulseSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleContactSplitPenetrationImpulseConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
+		}
+		return sum;
+	}
+};
+
+void btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+{
+	BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
+	if (infoGlobal.m_splitImpulse)
+	{
+		for (int iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
+		{
+			btScalar leastSquaresResidual = 0.f;
+			if (m_useBatching)
+			{
+				const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+				ContactSplitPenetrationImpulseSolverLoop loop(this, &batchedCons);
+				btScalar leastSquaresResidual = 0.f;
+				for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+				{
+					int iPhase = batchedCons.m_phaseOrder[iiPhase];
+					const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+					int grainSize = batchedCons.m_phaseGrainSize[iPhase];
+					leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+				}
+			}
+			else
+			{
+				// non-batched
+				leastSquaresResidual = resolveMultipleContactSplitPenetrationImpulseConstraints(m_orderTmpConstraintPool, 0, m_tmpSolverContactConstraintPool.size());
+			}
+			if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
+			{
+#ifdef VERBOSE_RESIDUAL_PRINTF
+				printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
+#endif
+				break;
+			}
+		}
+	}
+}
+
+btScalar btSequentialImpulseConstraintSolverMt::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+{
+	if (!m_useBatching)
+	{
+		return btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+	}
+	BT_PROFILE("solveSingleIterationMt");
+	btScalar leastSquaresResidual = 0.f;
+
+	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
+	{
+		if (1)  // uncomment this for a bit less random ((iteration & 7) == 0)
+		{
+			randomizeConstraintOrdering(iteration, infoGlobal.m_numIterations);
+		}
+	}
+
+	{
+		///solve all joint constraints
+		leastSquaresResidual += resolveAllJointConstraints(iteration);
+
+		if (iteration < infoGlobal.m_numIterations)
+		{
+			// this loop is only used for cone-twist constraints,
+			// it would be nice to skip this loop if none of the constraints need it
+			if (m_useObsoleteJointConstraints)
+			{
+				for (int j = 0; j < numConstraints; j++)
+				{
+					if (constraints[j]->isEnabled())
+					{
+						int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(), infoGlobal.m_timeStep);
+						int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(), infoGlobal.m_timeStep);
+						btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+						btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+						constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
+					}
+				}
+			}
+
+			if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
+			{
+				// solve all contact, contact-friction, and rolling friction constraints interleaved
+				leastSquaresResidual += resolveAllContactConstraintsInterleaved();
+			}
+			else  //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
+			{
+				// don't interleave them
+				// solve all contact constraints
+				leastSquaresResidual += resolveAllContactConstraints();
+
+				// solve all contact friction constraints
+				leastSquaresResidual += resolveAllContactFrictionConstraints();
+
+				// solve all rolling friction constraints
+				leastSquaresResidual += resolveAllRollingFrictionConstraints();
+			}
+		}
+	}
+	return leastSquaresResidual;
+}
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleJointConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration)
+{
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
+	{
+		int iCons = consIndices[iiCons];
+		const btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[iCons];
+		if (iteration < constraint.m_overrideNumSolverIterations)
+		{
+			btSolverBody& bodyA = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+			btSolverBody& bodyB = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+			btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, constraint);
+			leastSquaresResidual += residual * residual;
+		}
+	}
+	return leastSquaresResidual;
+}
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
+{
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
+	{
+		int iCons = consIndices[iiCons];
+		const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iCons];
+		btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
+		btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
+		btScalar residual = resolveSingleConstraintRowLowerLimit(bodyA, bodyB, solveManifold);
+		leastSquaresResidual += residual * residual;
+	}
+	return leastSquaresResidual;
+}
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
+{
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
+	{
+		int iContact = consIndices[iiCons];
+		btScalar totalImpulse = m_tmpSolverContactConstraintPool[iContact].m_appliedImpulse;
+
+		// apply sliding friction
+		if (totalImpulse > 0.0f)
+		{
+			int iBegin = iContact * m_numFrictionDirections;
+			int iEnd = iBegin + m_numFrictionDirections;
+			for (int iFriction = iBegin; iFriction < iEnd; ++iFriction)
+			{
+				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[iFriction++];
+				btAssert(solveManifold.m_frictionIndex == iContact);
+
+				solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+				solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
+
+				btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
+				btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
+				btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, solveManifold);
+				leastSquaresResidual += residual * residual;
+			}
+		}
+	}
+	return leastSquaresResidual;
+}
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactRollingFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
+{
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
+	{
+		int iContact = consIndices[iiCons];
+		int iFirstRollingFriction = m_rollingFrictionIndexTable[iContact];
+		if (iFirstRollingFriction >= 0)
+		{
+			btScalar totalImpulse = m_tmpSolverContactConstraintPool[iContact].m_appliedImpulse;
+			// apply rolling friction
+			if (totalImpulse > 0.0f)
+			{
+				int iBegin = iFirstRollingFriction;
+				int iEnd = iBegin + 3;
+				for (int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric)
+				{
+					btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric];
+					if (rollingFrictionConstraint.m_frictionIndex != iContact)
+					{
+						break;
+					}
+					btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
+					if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
+					{
+						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+					}
+
+					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+					btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
+					leastSquaresResidual += residual * residual;
+				}
+			}
+		}
+	}
+	return leastSquaresResidual;
+}
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraintsInterleaved(const btAlignedObjectArray<int>& contactIndices,
+																							 int batchBegin,
+																							 int batchEnd)
+{
+	btScalar leastSquaresResidual = 0.f;
+	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+
+	for (int iiCons = batchBegin; iiCons < batchEnd; iiCons++)
+	{
+		btScalar totalImpulse = 0;
+		int iContact = contactIndices[iiCons];
+		// apply penetration constraint
+		{
+			const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iContact];
+			btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+			leastSquaresResidual += residual * residual;
+			totalImpulse = solveManifold.m_appliedImpulse;
+		}
+
+		// apply sliding friction
+		if (totalImpulse > 0.0f)
+		{
+			int iBegin = iContact * m_numFrictionDirections;
+			int iEnd = iBegin + m_numFrictionDirections;
+			for (int iFriction = iBegin; iFriction < iEnd; ++iFriction)
+			{
+				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[iFriction];
+				btAssert(solveManifold.m_frictionIndex == iContact);
+
+				solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+				solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
+
+				btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
+				btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
+				btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, solveManifold);
+				leastSquaresResidual += residual * residual;
+			}
+		}
+
+		// apply rolling friction
+		int iFirstRollingFriction = m_rollingFrictionIndexTable[iContact];
+		if (totalImpulse > 0.0f && iFirstRollingFriction >= 0)
+		{
+			int iBegin = iFirstRollingFriction;
+			int iEnd = iBegin + 3;
+			for (int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric)
+			{
+				btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric];
+				if (rollingFrictionConstraint.m_frictionIndex != iContact)
+				{
+					break;
+				}
+				btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
+				if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
+				{
+					rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+				}
+
+				rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+				rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+				btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
+				leastSquaresResidual += residual * residual;
+			}
+		}
+	}
+	return leastSquaresResidual;
+}
+
+void btSequentialImpulseConstraintSolverMt::randomizeBatchedConstraintOrdering(btBatchedConstraints* batchedConstraints)
+{
+	btBatchedConstraints& bc = *batchedConstraints;
+	// randomize ordering of phases
+	for (int ii = 1; ii < bc.m_phaseOrder.size(); ++ii)
+	{
+		int iSwap = btRandInt2(ii + 1);
+		bc.m_phaseOrder.swap(ii, iSwap);
+	}
+
+	// for each batch,
+	for (int iBatch = 0; iBatch < bc.m_batches.size(); ++iBatch)
+	{
+		// randomize ordering of constraints within the batch
+		const btBatchedConstraints::Range& batch = bc.m_batches[iBatch];
+		for (int iiCons = batch.begin; iiCons < batch.end; ++iiCons)
+		{
+			int iSwap = batch.begin + btRandInt2(iiCons - batch.begin + 1);
+			btAssert(iSwap >= batch.begin && iSwap < batch.end);
+			bc.m_constraintIndices.swap(iiCons, iSwap);
+		}
+	}
+}
+
+void btSequentialImpulseConstraintSolverMt::randomizeConstraintOrdering(int iteration, int numIterations)
+{
+	// randomize ordering of joint constraints
+	randomizeBatchedConstraintOrdering(&m_batchedJointConstraints);
+
+	//contact/friction constraints are not solved more than numIterations
+	if (iteration < numIterations)
+	{
+		randomizeBatchedConstraintOrdering(&m_batchedContactConstraints);
+	}
+}
+
+struct JointSolverLoop : public btIParallelSumBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
+	int m_iteration;
+
+	JointSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, int iteration)
+	{
+		m_solver = solver;
+		m_bc = bc;
+		m_iteration = iteration;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("JointSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleJointConstraints(m_bc->m_constraintIndices, batch.begin, batch.end, m_iteration);
+		}
+		return sum;
+	}
+};
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveAllJointConstraints(int iteration)
+{
+	BT_PROFILE("resolveAllJointConstraints");
+	const btBatchedConstraints& batchedCons = m_batchedJointConstraints;
+	JointSolverLoop loop(this, &batchedCons, iteration);
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+	{
+		int iPhase = batchedCons.m_phaseOrder[iiPhase];
+		const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+		int grainSize = 1;
+		leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+	}
+	return leastSquaresResidual;
+}
+
+struct ContactSolverLoop : public btIParallelSumBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
+
+	ContactSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
+	{
+		m_solver = solver;
+		m_bc = bc;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("ContactSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleContactConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
+		}
+		return sum;
+	}
+};
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraints()
+{
+	BT_PROFILE("resolveAllContactConstraints");
+	const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+	ContactSolverLoop loop(this, &batchedCons);
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+	{
+		int iPhase = batchedCons.m_phaseOrder[iiPhase];
+		const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+		int grainSize = batchedCons.m_phaseGrainSize[iPhase];
+		leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+	}
+	return leastSquaresResidual;
+}
+
+struct ContactFrictionSolverLoop : public btIParallelSumBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
+
+	ContactFrictionSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
+	{
+		m_solver = solver;
+		m_bc = bc;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("ContactFrictionSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleContactFrictionConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
+		}
+		return sum;
+	}
+};
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactFrictionConstraints()
+{
+	BT_PROFILE("resolveAllContactFrictionConstraints");
+	const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+	ContactFrictionSolverLoop loop(this, &batchedCons);
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+	{
+		int iPhase = batchedCons.m_phaseOrder[iiPhase];
+		const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+		int grainSize = batchedCons.m_phaseGrainSize[iPhase];
+		leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+	}
+	return leastSquaresResidual;
+}
+
+struct InterleavedContactSolverLoop : public btIParallelSumBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
+
+	InterleavedContactSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
+	{
+		m_solver = solver;
+		m_bc = bc;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("InterleavedContactSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleContactConstraintsInterleaved(m_bc->m_constraintIndices, batch.begin, batch.end);
+		}
+		return sum;
+	}
+};
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraintsInterleaved()
+{
+	BT_PROFILE("resolveAllContactConstraintsInterleaved");
+	const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+	InterleavedContactSolverLoop loop(this, &batchedCons);
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+	{
+		int iPhase = batchedCons.m_phaseOrder[iiPhase];
+		const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+		int grainSize = 1;
+		leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+	}
+	return leastSquaresResidual;
+}
+
+struct ContactRollingFrictionSolverLoop : public btIParallelSumBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
+
+	ContactRollingFrictionSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
+	{
+		m_solver = solver;
+		m_bc = bc;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("ContactFrictionSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleContactRollingFrictionConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
+		}
+		return sum;
+	}
+};
+
+btScalar btSequentialImpulseConstraintSolverMt::resolveAllRollingFrictionConstraints()
+{
+	BT_PROFILE("resolveAllRollingFrictionConstraints");
+	btScalar leastSquaresResidual = 0.f;
+	//
+	// We do not generate batches for rolling friction constraints. We assume that
+	// one of two cases is true:
+	//
+	//  1. either most bodies in the simulation have rolling friction, in which case we can use the
+	//     batches for contacts and use a lookup table to translate contact indices to rolling friction
+	//     (ignoring any contact indices that don't map to a rolling friction constraint). As long as
+	//     most contacts have a corresponding rolling friction constraint, this should parallelize well.
+	//
+	//  -OR-
+	//
+	//  2. few bodies in the simulation have rolling friction, so it is not worth trying to use the
+	//     batches from contacts as most of the contacts won't have corresponding rolling friction
+	//     constraints and most threads would end up doing very little work. Most of the time would
+	//     go to threading overhead, so we don't bother with threading.
+	//
+	int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+	if (numRollingFrictionPoolConstraints >= m_tmpSolverContactConstraintPool.size())
+	{
+		// use batching if there are many rolling friction constraints
+		const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+		ContactRollingFrictionSolverLoop loop(this, &batchedCons);
+		btScalar leastSquaresResidual = 0.f;
+		for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+		{
+			int iPhase = batchedCons.m_phaseOrder[iiPhase];
+			const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+			int grainSize = 1;
+			leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+		}
+	}
+	else
+	{
+		// no batching, also ignores SOLVER_RANDMIZE_ORDER
+		for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
+		{
+			btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+			if (rollingFrictionConstraint.m_frictionIndex >= 0)
+			{
+				btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+				if (totalImpulse > 0.0f)
+				{
+					btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
+					if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
+						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+
+					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+					btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
+					leastSquaresResidual += residual * residual;
+				}
+			}
+		}
+	}
+	return leastSquaresResidual;
+}
+
+void btSequentialImpulseConstraintSolverMt::internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("internalWriteBackContacts");
+	writeBackContacts(iBegin, iEnd, infoGlobal);
+	//for ( int iContact = iBegin; iContact < iEnd; ++iContact)
+	//{
+	//    const btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[ iContact ];
+	//    btManifoldPoint* pt = (btManifoldPoint*) contactConstraint.m_originalContactPoint;
+	//    btAssert( pt );
+	//    pt->m_appliedImpulse = contactConstraint.m_appliedImpulse;
+	//    pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex ].m_appliedImpulse;
+	//    if ( m_numFrictionDirections == 2 )
+	//    {
+	//        pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex + 1 ].m_appliedImpulse;
+	//    }
+	//}
+}
+
+void btSequentialImpulseConstraintSolverMt::internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("internalWriteBackJoints");
+	writeBackJoints(iBegin, iEnd, infoGlobal);
+}
+
+void btSequentialImpulseConstraintSolverMt::internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("internalWriteBackBodies");
+	writeBackBodies(iBegin, iEnd, infoGlobal);
+}
+
+struct WriteContactPointsLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btContactSolverInfo* m_infoGlobal;
+
+	WriteContactPointsLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal)
+	{
+		m_solver = solver;
+		m_infoGlobal = &infoGlobal;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalWriteBackContacts(iBegin, iEnd, *m_infoGlobal);
+	}
+};
+
+struct WriteJointsLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btContactSolverInfo* m_infoGlobal;
+
+	WriteJointsLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal)
+	{
+		m_solver = solver;
+		m_infoGlobal = &infoGlobal;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalWriteBackJoints(iBegin, iEnd, *m_infoGlobal);
+	}
+};
+
+struct WriteBodiesLoop : public btIParallelForBody
+{
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btContactSolverInfo* m_infoGlobal;
+
+	WriteBodiesLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal)
+	{
+		m_solver = solver;
+		m_infoGlobal = &infoGlobal;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalWriteBackBodies(iBegin, iEnd, *m_infoGlobal);
+	}
+};
+
+btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+	BT_PROFILE("solveGroupCacheFriendlyFinish");
+
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	{
+		WriteContactPointsLoop loop(this, infoGlobal);
+		int grainSize = 500;
+		btParallelFor(0, m_tmpSolverContactConstraintPool.size(), grainSize, loop);
+	}
+
+	{
+		WriteJointsLoop loop(this, infoGlobal);
+		int grainSize = 400;
+		btParallelFor(0, m_tmpSolverNonContactConstraintPool.size(), grainSize, loop);
+	}
+	{
+		WriteBodiesLoop loop(this, infoGlobal);
+		int grainSize = 100;
+		btParallelFor(0, m_tmpSolverBodyPool.size(), grainSize, loop);
+	}
+
+	m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
+
+	m_tmpSolverBodyPool.resizeNoInitialize(0);
+	return 0.f;
+}
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h
new file mode 100644
index 00000000000..1861ddd7d76
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h
@@ -0,0 +1,150 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
+#define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
+
+#include "btSequentialImpulseConstraintSolver.h"
+#include "btBatchedConstraints.h"
+#include "LinearMath/btThreads.h"
+
+///
+/// btSequentialImpulseConstraintSolverMt
+///
+///  A multithreaded variant of the sequential impulse constraint solver. The constraints to be solved are grouped into
+///  batches and phases where each batch of constraints within a given phase can be solved in parallel with the rest.
+///  Ideally we want as few phases as possible, and each phase should have many batches, and all of the batches should
+///  have about the same number of constraints.
+///  This method works best on a large island of many constraints.
+///
+///  Supports all of the features of the normal sequential impulse solver such as:
+///    - split penetration impulse
+///    - rolling friction
+///    - interleaving constraints
+///    - warmstarting
+///    - 2 friction directions
+///    - randomized constraint ordering
+///    - early termination when leastSquaresResidualThreshold is satisfied
+///
+///  When the SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS flag is enabled, unlike the normal SequentialImpulse solver,
+///  the rolling friction is interleaved as well.
+///  Interleaving the contact penetration constraints with friction reduces the number of parallel loops that need to be done,
+///  which reduces threading overhead so it can be a performance win, however, it does seem to produce a less stable simulation,
+///  at least on stacks of blocks.
+///
+///  When the SOLVER_RANDMIZE_ORDER flag is enabled, the ordering of phases, and the ordering of constraints within each batch
+///  is randomized, however it does not swap constraints between batches.
+///  This is to avoid regenerating the batches for each solver iteration which would be quite costly in performance.
+///
+///  Note that a non-zero leastSquaresResidualThreshold could possibly affect the determinism of the simulation
+///  if the task scheduler's parallelSum operation is non-deterministic. The parallelSum operation can be non-deterministic
+///  because floating point addition is not associative due to rounding errors.
+///  The task scheduler can and should ensure that the result of any parallelSum operation is deterministic.
+///
+ATTRIBUTE_ALIGNED16(class)
+btSequentialImpulseConstraintSolverMt : public btSequentialImpulseConstraintSolver
+{
+public:
+	virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) BT_OVERRIDE;
+	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) BT_OVERRIDE;
+	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) BT_OVERRIDE;
+	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
+
+	// temp struct used to collect info from persistent manifolds into a cache-friendly struct using multiple threads
+	struct btContactManifoldCachedInfo
+	{
+		static const int MAX_NUM_CONTACT_POINTS = 4;
+
+		int numTouchingContacts;
+		int solverBodyIds[2];
+		int contactIndex;
+		int rollingFrictionIndex;
+		bool contactHasRollingFriction[MAX_NUM_CONTACT_POINTS];
+		btManifoldPoint* contactPoints[MAX_NUM_CONTACT_POINTS];
+	};
+	// temp struct used for setting up joint constraints in parallel
+	struct JointParams
+	{
+		int m_solverConstraint;
+		int m_solverBodyA;
+		int m_solverBodyB;
+	};
+	void internalInitMultipleJoints(btTypedConstraint * *constraints, int iBegin, int iEnd);
+	void internalConvertMultipleJoints(const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+
+	// parameters to control batching
+	static bool s_allowNestedParallelForLoops;        // whether to allow nested parallel operations
+	static int s_minimumContactManifoldsForBatching;  // don't even try to batch if fewer manifolds than this
+	static btBatchedConstraints::BatchingMethod s_contactBatchingMethod;
+	static btBatchedConstraints::BatchingMethod s_jointBatchingMethod;
+	static int s_minBatchSize;  // desired number of constraints per batch
+	static int s_maxBatchSize;
+
+protected:
+	static const int CACHE_LINE_SIZE = 64;
+
+	btBatchedConstraints m_batchedContactConstraints;
+	btBatchedConstraints m_batchedJointConstraints;
+	int m_numFrictionDirections;
+	bool m_useBatching;
+	bool m_useObsoleteJointConstraints;
+	btAlignedObjectArray<btContactManifoldCachedInfo> m_manifoldCachedInfoArray;
+	btAlignedObjectArray<int> m_rollingFrictionIndexTable;  // lookup table mapping contact index to rolling friction index
+	btSpinMutex m_bodySolverArrayMutex;
+	char m_antiFalseSharingPadding[CACHE_LINE_SIZE];  // padding to keep mutexes in separate cachelines
+	btSpinMutex m_kinematicBodyUniqueIdToSolverBodyTableMutex;
+	btAlignedObjectArray<char> m_scratchMemory;
+
+	virtual void randomizeConstraintOrdering(int iteration, int numIterations);
+	virtual btScalar resolveAllJointConstraints(int iteration);
+	virtual btScalar resolveAllContactConstraints();
+	virtual btScalar resolveAllContactFrictionConstraints();
+	virtual btScalar resolveAllContactConstraintsInterleaved();
+	virtual btScalar resolveAllRollingFrictionConstraints();
+
+	virtual void setupBatchedContactConstraints();
+	virtual void setupBatchedJointConstraints();
+	virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
+	virtual void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
+	virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE;
+
+	int getOrInitSolverBodyThreadsafe(btCollisionObject & body, btScalar timeStep);
+	void allocAllContactConstraints(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
+	void setupAllContactConstraints(const btContactSolverInfo& infoGlobal);
+	void randomizeBatchedConstraintOrdering(btBatchedConstraints * batchedConstraints);
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btSequentialImpulseConstraintSolverMt();
+	virtual ~btSequentialImpulseConstraintSolverMt();
+
+	btScalar resolveMultipleJointConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration);
+	btScalar resolveMultipleContactConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
+	btScalar resolveMultipleContactSplitPenetrationImpulseConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
+	btScalar resolveMultipleContactFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
+	btScalar resolveMultipleContactRollingFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd);
+	btScalar resolveMultipleContactConstraintsInterleaved(const btAlignedObjectArray<int>& contactIndices, int batchBegin, int batchEnd);
+
+	void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo * cachedInfoArray, btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
+	void internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds);
+	void internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal);
+	void internalConvertBodies(btCollisionObject * *bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+	void internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+	void internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+	void internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+};
+
+#endif  //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
index f8f81bfe6fa..cac5302a73e 100644..100755
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
@@ -18,8 +18,6 @@ Added by Roman Ponomarev (rponom@gmail.com)
 April 04, 2008
 */
 
-
-
 #include "btSliderConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
@@ -29,10 +27,10 @@ April 04, 2008
 
 void btSliderConstraint::initParams()
 {
-    m_lowerLinLimit = btScalar(1.0);
-    m_upperLinLimit = btScalar(-1.0);
-    m_lowerAngLimit = btScalar(0.);
-    m_upperAngLimit = btScalar(0.);
+	m_lowerLinLimit = btScalar(1.0);
+	m_upperLinLimit = btScalar(-1.0);
+	m_lowerAngLimit = btScalar(0.);
+	m_upperAngLimit = btScalar(0.);
 	m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
 	m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
 	m_dampingDirLin = btScalar(0.);
@@ -59,13 +57,13 @@ void btSliderConstraint::initParams()
 	m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM;
 
 	m_poweredLinMotor = false;
-    m_targetLinMotorVelocity = btScalar(0.);
-    m_maxLinMotorForce = btScalar(0.);
+	m_targetLinMotorVelocity = btScalar(0.);
+	m_maxLinMotorForce = btScalar(0.);
 	m_accumulatedLinMotorImpulse = btScalar(0.0);
 
 	m_poweredAngMotor = false;
-    m_targetAngMotorVelocity = btScalar(0.);
-    m_maxAngMotorForce = btScalar(0.);
+	m_targetAngMotorVelocity = btScalar(0.);
+	m_maxAngMotorForce = btScalar(0.);
 	m_accumulatedAngMotorImpulse = btScalar(0.0);
 
 	m_flags = 0;
@@ -73,43 +71,32 @@ void btSliderConstraint::initParams()
 
 	m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME;
 
-	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 }
 
-
-
-
-
 btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
-        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB),
-		m_useSolveConstraintObsolete(false),
-		m_frameInA(frameInA),
-        m_frameInB(frameInB),
-		m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+	: btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB),
+	  m_useSolveConstraintObsolete(false),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB),
+	  m_useLinearReferenceFrameA(useLinearReferenceFrameA)
 {
 	initParams();
 }
 
-
-
 btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA)
-        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB),
-		m_useSolveConstraintObsolete(false),
-		m_frameInB(frameInB),
-		m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+	: btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB),
+	  m_useSolveConstraintObsolete(false),
+	  m_frameInB(frameInB),
+	  m_useLinearReferenceFrameA(useLinearReferenceFrameA)
 {
 	///not providing rigidbody A means implicitly using worldspace for body A
 	m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
-//	m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());
+	//	m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());
 
 	initParams();
 }
 
-
-
-
-
-
 void btSliderConstraint::getInfo1(btConstraintInfo1* info)
 {
 	if (m_useSolveConstraintObsolete)
@@ -119,46 +106,39 @@ void btSliderConstraint::getInfo1(btConstraintInfo1* info)
 	}
 	else
 	{
-		info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
-		info->nub = 2; 
+		info->m_numConstraintRows = 4;  // Fixed 2 linear + 2 angular
+		info->nub = 2;
 		//prepare constraint
-		calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+		calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 		testAngLimits();
 		testLinLimits();
-		if(getSolveLinLimit() || getPoweredLinMotor())
+		if (getSolveLinLimit() || getPoweredLinMotor())
 		{
-			info->m_numConstraintRows++; // limit 3rd linear as well
-			info->nub--; 
+			info->m_numConstraintRows++;  // limit 3rd linear as well
+			info->nub--;
 		}
-		if(getSolveAngLimit() || getPoweredAngMotor())
+		if (getSolveAngLimit() || getPoweredAngMotor())
 		{
-			info->m_numConstraintRows++; // limit 3rd angular as well
-			info->nub--; 
+			info->m_numConstraintRows++;  // limit 3rd angular as well
+			info->nub--;
 		}
 	}
 }
 
 void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
 {
-
-	info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used)
-	info->nub = 0; 
+	info->m_numConstraintRows = 6;  // Fixed 2 linear + 2 angular + 1 limit (even if not used)
+	info->nub = 0;
 }
 
 void btSliderConstraint::getInfo2(btConstraintInfo2* info)
 {
-	getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
+	getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(), m_rbB.getLinearVelocity(), m_rbA.getInvMass(), m_rbB.getInvMass());
 }
 
-
-
-
-
-
-
-void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+void btSliderConstraint::calculateTransforms(const btTransform& transA, const btTransform& transB)
 {
-	if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
+	if (m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
 	{
 		m_calculatedTransformA = transA * m_frameInA;
 		m_calculatedTransformB = transB * m_frameInB;
@@ -170,8 +150,8 @@ void btSliderConstraint::calculateTransforms(const btTransform& transA,const btT
 	}
 	m_realPivotAInW = m_calculatedTransformA.getOrigin();
 	m_realPivotBInW = m_calculatedTransformB.getOrigin();
-	m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
-	if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
+	m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0);  // along X
+	if (m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
 	{
 		m_delta = m_realPivotBInW - m_realPivotAInW;
 	}
@@ -180,30 +160,28 @@ void btSliderConstraint::calculateTransforms(const btTransform& transA,const btT
 		m_delta = m_realPivotAInW - m_realPivotBInW;
 	}
 	m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
-    btVector3 normalWorld;
-    int i;
-    //linear part
-    for(i = 0; i < 3; i++)
-    {
+	btVector3 normalWorld;
+	int i;
+	//linear part
+	for (i = 0; i < 3; i++)
+	{
 		normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
 		m_depth[i] = m_delta.dot(normalWorld);
-    }
+	}
 }
- 
-
 
 void btSliderConstraint::testLinLimits(void)
 {
 	m_solveLinLim = false;
 	m_linPos = m_depth[0];
-	if(m_lowerLinLimit <= m_upperLinLimit)
+	if (m_lowerLinLimit <= m_upperLinLimit)
 	{
-		if(m_depth[0] > m_upperLinLimit)
+		if (m_depth[0] > m_upperLinLimit)
 		{
 			m_depth[0] -= m_upperLinLimit;
 			m_solveLinLim = true;
 		}
-		else if(m_depth[0] < m_lowerLinLimit)
+		else if (m_depth[0] < m_lowerLinLimit)
 		{
 			m_depth[0] -= m_lowerLinLimit;
 			m_solveLinLim = true;
@@ -219,27 +197,25 @@ void btSliderConstraint::testLinLimits(void)
 	}
 }
 
-
-
 void btSliderConstraint::testAngLimits(void)
 {
 	m_angDepth = btScalar(0.);
 	m_solveAngLim = false;
-	if(m_lowerAngLimit <= m_upperAngLimit)
+	if (m_lowerAngLimit <= m_upperAngLimit)
 	{
 		const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
 		const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
 		const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
-//		btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));  
-		btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0));  
+		//		btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
+		btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0));
 		rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit);
 		m_angPos = rot;
-		if(rot < m_lowerAngLimit)
+		if (rot < m_lowerAngLimit)
 		{
 			m_angDepth = rot - m_lowerAngLimit;
 			m_solveAngLim = true;
-		} 
-		else if(rot > m_upperAngLimit)
+		}
+		else if (rot > m_upperAngLimit)
 		{
 			m_angDepth = rot - m_upperAngLimit;
 			m_solveAngLim = true;
@@ -255,8 +231,6 @@ btVector3 btSliderConstraint::getAncorInA(void)
 	return ancorInA;
 }
 
-
-
 btVector3 btSliderConstraint::getAncorInB(void)
 {
 	btVector3 ancorInB;
@@ -264,17 +238,16 @@ btVector3 btSliderConstraint::getAncorInB(void)
 	return ancorInB;
 }
 
-
-void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass  )
+void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, btScalar rbAinvMass, btScalar rbBinvMass)
 {
 	const btTransform& trA = getCalculatedTransformA();
 	const btTransform& trB = getCalculatedTransformB();
-	
+
 	btAssert(!m_useSolveConstraintObsolete);
 	int i, s = info->rowskip;
-	
+
 	btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
-	
+
 	// difference between frames in WCS
 	btVector3 ofs = trB.getOrigin() - trA.getOrigin();
 	// now get weight factors depending on masses
@@ -283,11 +256,11 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 	bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
 	btScalar miS = miA + miB;
 	btScalar factA, factB;
-	if(miS > btScalar(0.f))
+	if (miS > btScalar(0.f))
 	{
 		factA = miB / miS;
 	}
-	else 
+	else
 	{
 		factA = btScalar(0.5f);
 	}
@@ -295,17 +268,17 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 	btVector3 ax1, p, q;
 	btVector3 ax1A = trA.getBasis().getColumn(0);
 	btVector3 ax1B = trB.getBasis().getColumn(0);
-	if(m_useOffsetForConstraintFrame)
+	if (m_useOffsetForConstraintFrame)
 	{
 		// get the desired direction of slider axis
 		// as weighted sum of X-orthos of frameA and frameB in WCS
 		ax1 = ax1A * factA + ax1B * factB;
 		ax1.normalize();
 		// construct two orthos to slider axis
-		btPlaneSpace1 (ax1, p, q);
+		btPlaneSpace1(ax1, p, q);
 	}
 	else
-	{ // old way - use frameA
+	{  // old way - use frameA
 		ax1 = trA.getBasis().getColumn(0);
 		// get 2 orthos to slider axis (Y, Z)
 		p = trA.getBasis().getColumn(1);
@@ -322,16 +295,16 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 	info->m_J1angularAxis[0] = p[0];
 	info->m_J1angularAxis[1] = p[1];
 	info->m_J1angularAxis[2] = p[2];
-	info->m_J1angularAxis[s+0] = q[0];
-	info->m_J1angularAxis[s+1] = q[1];
-	info->m_J1angularAxis[s+2] = q[2];
+	info->m_J1angularAxis[s + 0] = q[0];
+	info->m_J1angularAxis[s + 1] = q[1];
+	info->m_J1angularAxis[s + 2] = q[2];
 
 	info->m_J2angularAxis[0] = -p[0];
 	info->m_J2angularAxis[1] = -p[1];
 	info->m_J2angularAxis[2] = -p[2];
-	info->m_J2angularAxis[s+0] = -q[0];
-	info->m_J2angularAxis[s+1] = -q[1];
-	info->m_J2angularAxis[s+2] = -q[2];
+	info->m_J2angularAxis[s + 0] = -q[0];
+	info->m_J2angularAxis[s + 1] = -q[1];
+	info->m_J2angularAxis[s + 2] = -q[2];
 	// compute the right hand side of the constraint equation. set relative
 	// body velocities along p and q to bring the slider back into alignment.
 	// if ax1A,ax1B are the unit length slider axes as computed from bodyA and
@@ -347,26 +320,25 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 	//    angular_velocity  = (erp*fps) * (ax1 x ax2)
 	// ax1 x ax2 is in the plane space of ax1, so we project the angular
 	// velocity to p and q to find the right hand side.
-//	btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
+	//	btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
 	btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
 	btScalar k = info->fps * currERP;
 
 	btVector3 u = ax1A.cross(ax1B);
 	info->m_constraintError[0] = k * u.dot(p);
 	info->m_constraintError[s] = k * u.dot(q);
-	if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
+	if (m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
 	{
 		info->cfm[0] = m_cfmOrthoAng;
 		info->cfm[s] = m_cfmOrthoAng;
 	}
 
-	int nrow = 1; // last filled row
+	int nrow = 1;  // last filled row
 	int srow;
 	btScalar limit_err;
 	int limit;
-	int powered;
 
-	// next two rows. 
+	// next two rows.
 	// we want: velA + wA x relA == velB + wB x relB ... but this would
 	// result in three equations, so we project along two orthos to the slider axis
 
@@ -376,8 +348,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 	int s2 = nrow * s;
 	nrow++;
 	int s3 = nrow * s;
-	btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0);
-	if(m_useOffsetForConstraintFrame)
+	btVector3 tmpA(0, 0, 0), tmpB(0, 0, 0), relA(0, 0, 0), relB(0, 0, 0), c(0, 0, 0);
+	if (m_useOffsetForConstraintFrame)
 	{
 		// get vector from bodyB to frameB in WCS
 		relB = trB.getOrigin() - bodyB_trans.getOrigin();
@@ -399,7 +371,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 		// now choose average ortho to slider axis
 		p = orthoB * factA + orthoA * factB;
 		btScalar len2 = p.length2();
-		if(len2 > SIMD_EPSILON)
+		if (len2 > SIMD_EPSILON)
 		{
 			p /= btSqrt(len2);
 		}
@@ -412,38 +384,38 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 		// fill two rows
 		tmpA = relA.cross(p);
 		tmpB = relB.cross(p);
-		for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
-		for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
+		for (i = 0; i < 3; i++) info->m_J1angularAxis[s2 + i] = tmpA[i];
+		for (i = 0; i < 3; i++) info->m_J2angularAxis[s2 + i] = -tmpB[i];
 		tmpA = relA.cross(q);
 		tmpB = relB.cross(q);
-		if(hasStaticBody && getSolveAngLimit())
-		{ // to make constraint between static and dynamic objects more rigid
+		if (hasStaticBody && getSolveAngLimit())
+		{  // to make constraint between static and dynamic objects more rigid
 			// remove wA (or wB) from equation if angular limit is hit
 			tmpB *= factB;
 			tmpA *= factA;
 		}
-		for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i];
-		for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i];
-		for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
-		for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
-		for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i];
-		for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i];
+		for (i = 0; i < 3; i++) info->m_J1angularAxis[s3 + i] = tmpA[i];
+		for (i = 0; i < 3; i++) info->m_J2angularAxis[s3 + i] = -tmpB[i];
+		for (i = 0; i < 3; i++) info->m_J1linearAxis[s2 + i] = p[i];
+		for (i = 0; i < 3; i++) info->m_J1linearAxis[s3 + i] = q[i];
+		for (i = 0; i < 3; i++) info->m_J2linearAxis[s2 + i] = -p[i];
+		for (i = 0; i < 3; i++) info->m_J2linearAxis[s3 + i] = -q[i];
 	}
 	else
-	{	// old way - maybe incorrect if bodies are not on the slider axis
+	{  // old way - maybe incorrect if bodies are not on the slider axis
 		// see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
 		c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
 		btVector3 tmp = c.cross(p);
-		for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
-		for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
+		for (i = 0; i < 3; i++) info->m_J1angularAxis[s2 + i] = factA * tmp[i];
+		for (i = 0; i < 3; i++) info->m_J2angularAxis[s2 + i] = factB * tmp[i];
 		tmp = c.cross(q);
-		for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
-		for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
+		for (i = 0; i < 3; i++) info->m_J1angularAxis[s3 + i] = factA * tmp[i];
+		for (i = 0; i < 3; i++) info->m_J2angularAxis[s3 + i] = factB * tmp[i];
 
-		for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
-		for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
-		for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i];
-		for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i];
+		for (i = 0; i < 3; i++) info->m_J1linearAxis[s2 + i] = p[i];
+		for (i = 0; i < 3; i++) info->m_J1linearAxis[s3 + i] = q[i];
+		for (i = 0; i < 3; i++) info->m_J2linearAxis[s2 + i] = -p[i];
+		for (i = 0; i < 3; i++) info->m_J2linearAxis[s3 + i] = -q[i];
 	}
 	// compute two elements of right hand side
 
@@ -455,37 +427,32 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 	info->m_constraintError[s2] = rhs;
 	rhs = k * q.dot(ofs);
 	info->m_constraintError[s3] = rhs;
-	if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
+	if (m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
 	{
 		info->cfm[s2] = m_cfmOrthoLin;
 		info->cfm[s3] = m_cfmOrthoLin;
 	}
 
-
 	// check linear limits
 	limit_err = btScalar(0.0);
 	limit = 0;
-	if(getSolveLinLimit())
+	if (getSolveLinLimit())
 	{
-		limit_err = getLinDepth() *  signFact;
+		limit_err = getLinDepth() * signFact;
 		limit = (limit_err > btScalar(0.0)) ? 2 : 1;
 	}
-	powered = 0;
-	if(getPoweredLinMotor())
-	{
-		powered = 1;
-	}
+	bool powered = getPoweredLinMotor();
 	// if the slider has joint limits or motor, add in the extra row
-	if (limit || powered) 
+	if (limit || powered)
 	{
 		nrow++;
 		srow = nrow * info->rowskip;
-		info->m_J1linearAxis[srow+0] = ax1[0];
-		info->m_J1linearAxis[srow+1] = ax1[1];
-		info->m_J1linearAxis[srow+2] = ax1[2];
-		info->m_J2linearAxis[srow+0] = -ax1[0];
-		info->m_J2linearAxis[srow+1] = -ax1[1];
-		info->m_J2linearAxis[srow+2] = -ax1[2];
+		info->m_J1linearAxis[srow + 0] = ax1[0];
+		info->m_J1linearAxis[srow + 1] = ax1[1];
+		info->m_J1linearAxis[srow + 2] = ax1[2];
+		info->m_J2linearAxis[srow + 0] = -ax1[0];
+		info->m_J2linearAxis[srow + 1] = -ax1[1];
+		info->m_J2linearAxis[srow + 2] = -ax1[2];
 		// linear torque decoupling step:
 		//
 		// we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
@@ -493,46 +460,46 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 		// constraint force is applied at must lie along the same ax1 axis.
 		// a torque couple will result in limited slider-jointed free
 		// bodies from gaining angular momentum.
-		if(m_useOffsetForConstraintFrame)
+		if (m_useOffsetForConstraintFrame)
 		{
 			// this is needed only when bodyA and bodyB are both dynamic.
-			if(!hasStaticBody)
+			if (!hasStaticBody)
 			{
 				tmpA = relA.cross(ax1);
 				tmpB = relB.cross(ax1);
-				info->m_J1angularAxis[srow+0] = tmpA[0];
-				info->m_J1angularAxis[srow+1] = tmpA[1];
-				info->m_J1angularAxis[srow+2] = tmpA[2];
-				info->m_J2angularAxis[srow+0] = -tmpB[0];
-				info->m_J2angularAxis[srow+1] = -tmpB[1];
-				info->m_J2angularAxis[srow+2] = -tmpB[2];
+				info->m_J1angularAxis[srow + 0] = tmpA[0];
+				info->m_J1angularAxis[srow + 1] = tmpA[1];
+				info->m_J1angularAxis[srow + 2] = tmpA[2];
+				info->m_J2angularAxis[srow + 0] = -tmpB[0];
+				info->m_J2angularAxis[srow + 1] = -tmpB[1];
+				info->m_J2angularAxis[srow + 2] = -tmpB[2];
 			}
 		}
 		else
-		{ // The old way. May be incorrect if bodies are not on the slider axis
-			btVector3 ltd;	// Linear Torque Decoupling vector (a torque)
+		{                   // The old way. May be incorrect if bodies are not on the slider axis
+			btVector3 ltd;  // Linear Torque Decoupling vector (a torque)
 			ltd = c.cross(ax1);
-			info->m_J1angularAxis[srow+0] = factA*ltd[0];
-			info->m_J1angularAxis[srow+1] = factA*ltd[1];
-			info->m_J1angularAxis[srow+2] = factA*ltd[2];
-			info->m_J2angularAxis[srow+0] = factB*ltd[0];
-			info->m_J2angularAxis[srow+1] = factB*ltd[1];
-			info->m_J2angularAxis[srow+2] = factB*ltd[2];
+			info->m_J1angularAxis[srow + 0] = factA * ltd[0];
+			info->m_J1angularAxis[srow + 1] = factA * ltd[1];
+			info->m_J1angularAxis[srow + 2] = factA * ltd[2];
+			info->m_J2angularAxis[srow + 0] = factB * ltd[0];
+			info->m_J2angularAxis[srow + 1] = factB * ltd[1];
+			info->m_J2angularAxis[srow + 2] = factB * ltd[2];
 		}
 		// right-hand part
 		btScalar lostop = getLowerLinLimit();
 		btScalar histop = getUpperLinLimit();
-		if(limit && (lostop == histop))
+		if (limit && (lostop == histop))
 		{  // the joint motor is ineffective
-			powered = 0;
+			powered = false;
 		}
 		info->m_constraintError[srow] = 0.;
 		info->m_lowerLimit[srow] = 0.;
 		info->m_upperLimit[srow] = 0.;
 		currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp;
-		if(powered)
+		if (powered)
 		{
-			if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
+			if (m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
 			{
 				info->cfm[srow] = m_cfmDirLin;
 			}
@@ -542,41 +509,41 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 			info->m_lowerLimit[srow] += -getMaxLinMotorForce() / info->fps;
 			info->m_upperLimit[srow] += getMaxLinMotorForce() / info->fps;
 		}
-		if(limit)
+		if (limit)
 		{
 			k = info->fps * currERP;
 			info->m_constraintError[srow] += k * limit_err;
-			if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
+			if (m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
 			{
 				info->cfm[srow] = m_cfmLimLin;
 			}
-			if(lostop == histop) 
-			{	// limited low and high simultaneously
+			if (lostop == histop)
+			{  // limited low and high simultaneously
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
 			}
-			else if(limit == 1) 
-			{ // low limit
+			else if (limit == 1)
+			{  // low limit
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = 0;
 			}
-			else 
-			{ // high limit
+			else
+			{  // high limit
 				info->m_lowerLimit[srow] = 0;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
 			}
 			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
 			btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
-			if(bounce > btScalar(0.0))
+			if (bounce > btScalar(0.0))
 			{
 				btScalar vel = linVelA.dot(ax1);
 				vel -= linVelB.dot(ax1);
 				vel *= signFact;
 				// only apply bounce if the velocity is incoming, and if the
 				// resulting c[] exceeds what we already have.
-				if(limit == 1)
-				{	// low limit
-					if(vel < 0)
+				if (limit == 1)
+				{  // low limit
+					if (vel < 0)
 					{
 						btScalar newc = -bounce * vel;
 						if (newc > info->m_constraintError[srow])
@@ -586,11 +553,11 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 					}
 				}
 				else
-				{ // high limit - all those computations are reversed
-					if(vel > 0)
+				{  // high limit - all those computations are reversed
+					if (vel > 0)
 					{
 						btScalar newc = -bounce * vel;
-						if(newc < info->m_constraintError[srow]) 
+						if (newc < info->m_constraintError[srow])
 						{
 							info->m_constraintError[srow] = newc;
 						}
@@ -598,44 +565,40 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 				}
 			}
 			info->m_constraintError[srow] *= getSoftnessLimLin();
-		} // if(limit)
-	} // if linear limit
+		}  // if(limit)
+	}      // if linear limit
 	// check angular limits
 	limit_err = btScalar(0.0);
 	limit = 0;
-	if(getSolveAngLimit())
+	if (getSolveAngLimit())
 	{
 		limit_err = getAngDepth();
 		limit = (limit_err > btScalar(0.0)) ? 1 : 2;
 	}
 	// if the slider has joint limits, add in the extra row
-	powered = 0;
-	if(getPoweredAngMotor())
-	{
-		powered = 1;
-	}
-	if(limit || powered) 
+	powered = getPoweredAngMotor();
+	if (limit || powered)
 	{
 		nrow++;
 		srow = nrow * info->rowskip;
-		info->m_J1angularAxis[srow+0] = ax1[0];
-		info->m_J1angularAxis[srow+1] = ax1[1];
-		info->m_J1angularAxis[srow+2] = ax1[2];
+		info->m_J1angularAxis[srow + 0] = ax1[0];
+		info->m_J1angularAxis[srow + 1] = ax1[1];
+		info->m_J1angularAxis[srow + 2] = ax1[2];
 
-		info->m_J2angularAxis[srow+0] = -ax1[0];
-		info->m_J2angularAxis[srow+1] = -ax1[1];
-		info->m_J2angularAxis[srow+2] = -ax1[2];
+		info->m_J2angularAxis[srow + 0] = -ax1[0];
+		info->m_J2angularAxis[srow + 1] = -ax1[1];
+		info->m_J2angularAxis[srow + 2] = -ax1[2];
 
 		btScalar lostop = getLowerAngLimit();
 		btScalar histop = getUpperAngLimit();
-		if(limit && (lostop == histop))
+		if (limit && (lostop == histop))
 		{  // the joint motor is ineffective
-			powered = 0;
+			powered = false;
 		}
 		currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp;
-		if(powered)
+		if (powered)
 		{
-			if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
+			if (m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
 			{
 				info->cfm[srow] = m_cfmDirAng;
 			}
@@ -644,55 +607,55 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 			info->m_lowerLimit[srow] = -getMaxAngMotorForce() / info->fps;
 			info->m_upperLimit[srow] = getMaxAngMotorForce() / info->fps;
 		}
-		if(limit)
+		if (limit)
 		{
 			k = info->fps * currERP;
 			info->m_constraintError[srow] += k * limit_err;
-			if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
+			if (m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
 			{
 				info->cfm[srow] = m_cfmLimAng;
 			}
-			if(lostop == histop) 
+			if (lostop == histop)
 			{
 				// limited low and high simultaneously
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
 			}
-			else if(limit == 1) 
-			{ // low limit
+			else if (limit == 1)
+			{  // low limit
 				info->m_lowerLimit[srow] = 0;
 				info->m_upperLimit[srow] = SIMD_INFINITY;
 			}
-			else 
-			{ // high limit
+			else
+			{  // high limit
 				info->m_lowerLimit[srow] = -SIMD_INFINITY;
 				info->m_upperLimit[srow] = 0;
 			}
 			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
 			btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
-			if(bounce > btScalar(0.0))
+			if (bounce > btScalar(0.0))
 			{
 				btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
 				vel -= m_rbB.getAngularVelocity().dot(ax1);
 				// only apply bounce if the velocity is incoming, and if the
 				// resulting c[] exceeds what we already have.
-				if(limit == 1)
-				{	// low limit
-					if(vel < 0)
+				if (limit == 1)
+				{  // low limit
+					if (vel < 0)
 					{
 						btScalar newc = -bounce * vel;
-						if(newc > info->m_constraintError[srow])
+						if (newc > info->m_constraintError[srow])
 						{
 							info->m_constraintError[srow] = newc;
 						}
 					}
 				}
 				else
-				{	// high limit - all those computations are reversed
-					if(vel > 0)
+				{  // high limit - all those computations are reversed
+					if (vel > 0)
 					{
 						btScalar newc = -bounce * vel;
-						if(newc < info->m_constraintError[srow])
+						if (newc < info->m_constraintError[srow])
 						{
 							info->m_constraintError[srow] = newc;
 						}
@@ -700,165 +663,161 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 				}
 			}
 			info->m_constraintError[srow] *= getSoftnessLimAng();
-		} // if(limit)
-	} // if angular limit or powered
+		}  // if(limit)
+	}      // if angular limit or powered
 }
 
-
-///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 ///If no axis is provided, it uses the default axis for this constraint.
 void btSliderConstraint::setParam(int num, btScalar value, int axis)
 {
-	switch(num)
+	switch (num)
 	{
-	case BT_CONSTRAINT_STOP_ERP :
-		if(axis < 1)
-		{
-			m_softnessLimLin = value;
-			m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN;
-		}
-		else if(axis < 3)
-		{
-			m_softnessOrthoLin = value;
-			m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN;
-		}
-		else if(axis == 3)
-		{
-			m_softnessLimAng = value;
-			m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG;
-		}
-		else if(axis < 6)
-		{
-			m_softnessOrthoAng = value;
-			m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG;
-		}
-		else
-		{
-			btAssertConstrParams(0);
-		}
-		break;
-	case BT_CONSTRAINT_CFM :
-		if(axis < 1)
-		{
-			m_cfmDirLin = value;
-			m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN;
-		}
-		else if(axis == 3)
-		{
-			m_cfmDirAng = value;
-			m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG;
-		}
-		else
-		{
-			btAssertConstrParams(0);
-		}
-		break;
-	case BT_CONSTRAINT_STOP_CFM :
-		if(axis < 1)
-		{
-			m_cfmLimLin = value;
-			m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN;
-		}
-		else if(axis < 3)
-		{
-			m_cfmOrthoLin = value;
-			m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN;
-		}
-		else if(axis == 3)
-		{
-			m_cfmLimAng = value;
-			m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG;
-		}
-		else if(axis < 6)
-		{
-			m_cfmOrthoAng = value;
-			m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG;
-		}
-		else
-		{
-			btAssertConstrParams(0);
-		}
-		break;
+		case BT_CONSTRAINT_STOP_ERP:
+			if (axis < 1)
+			{
+				m_softnessLimLin = value;
+				m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN;
+			}
+			else if (axis < 3)
+			{
+				m_softnessOrthoLin = value;
+				m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN;
+			}
+			else if (axis == 3)
+			{
+				m_softnessLimAng = value;
+				m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG;
+			}
+			else if (axis < 6)
+			{
+				m_softnessOrthoAng = value;
+				m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG;
+			}
+			else
+			{
+				btAssertConstrParams(0);
+			}
+			break;
+		case BT_CONSTRAINT_CFM:
+			if (axis < 1)
+			{
+				m_cfmDirLin = value;
+				m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN;
+			}
+			else if (axis == 3)
+			{
+				m_cfmDirAng = value;
+				m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG;
+			}
+			else
+			{
+				btAssertConstrParams(0);
+			}
+			break;
+		case BT_CONSTRAINT_STOP_CFM:
+			if (axis < 1)
+			{
+				m_cfmLimLin = value;
+				m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN;
+			}
+			else if (axis < 3)
+			{
+				m_cfmOrthoLin = value;
+				m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN;
+			}
+			else if (axis == 3)
+			{
+				m_cfmLimAng = value;
+				m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG;
+			}
+			else if (axis < 6)
+			{
+				m_cfmOrthoAng = value;
+				m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG;
+			}
+			else
+			{
+				btAssertConstrParams(0);
+			}
+			break;
 	}
 }
 
 ///return the local value of parameter
-btScalar btSliderConstraint::getParam(int num, int axis) const 
+btScalar btSliderConstraint::getParam(int num, int axis) const
 {
 	btScalar retVal(SIMD_INFINITY);
-	switch(num)
+	switch (num)
 	{
-	case BT_CONSTRAINT_STOP_ERP :
-		if(axis < 1)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN);
-			retVal = m_softnessLimLin;
-		}
-		else if(axis < 3)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN);
-			retVal = m_softnessOrthoLin;
-		}
-		else if(axis == 3)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG);
-			retVal = m_softnessLimAng;
-		}
-		else if(axis < 6)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG);
-			retVal = m_softnessOrthoAng;
-		}
-		else
-		{
-			btAssertConstrParams(0);
-		}
-		break;
-	case BT_CONSTRAINT_CFM :
-		if(axis < 1)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN);
-			retVal = m_cfmDirLin;
-		}
-		else if(axis == 3)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG);
-			retVal = m_cfmDirAng;
-		}
-		else
-		{
-			btAssertConstrParams(0);
-		}
-		break;
-	case BT_CONSTRAINT_STOP_CFM :
-		if(axis < 1)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN);
-			retVal = m_cfmLimLin;
-		}
-		else if(axis < 3)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN);
-			retVal = m_cfmOrthoLin;
-		}
-		else if(axis == 3)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG);
-			retVal = m_cfmLimAng;
-		}
-		else if(axis < 6)
-		{
-			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG);
-			retVal = m_cfmOrthoAng;
-		}
-		else
-		{
-			btAssertConstrParams(0);
-		}
-		break;
+		case BT_CONSTRAINT_STOP_ERP:
+			if (axis < 1)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN);
+				retVal = m_softnessLimLin;
+			}
+			else if (axis < 3)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN);
+				retVal = m_softnessOrthoLin;
+			}
+			else if (axis == 3)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG);
+				retVal = m_softnessLimAng;
+			}
+			else if (axis < 6)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG);
+				retVal = m_softnessOrthoAng;
+			}
+			else
+			{
+				btAssertConstrParams(0);
+			}
+			break;
+		case BT_CONSTRAINT_CFM:
+			if (axis < 1)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN);
+				retVal = m_cfmDirLin;
+			}
+			else if (axis == 3)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG);
+				retVal = m_cfmDirAng;
+			}
+			else
+			{
+				btAssertConstrParams(0);
+			}
+			break;
+		case BT_CONSTRAINT_STOP_CFM:
+			if (axis < 1)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN);
+				retVal = m_cfmLimLin;
+			}
+			else if (axis < 3)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN);
+				retVal = m_cfmOrthoLin;
+			}
+			else if (axis == 3)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG);
+				retVal = m_cfmLimAng;
+			}
+			else if (axis < 6)
+			{
+				btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG);
+				retVal = m_cfmOrthoAng;
+			}
+			else
+			{
+				btAssertConstrParams(0);
+			}
+			break;
 	}
 	return retVal;
 }
-
-
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
index 628ada4cfb1..75ca34e978f 100644..100755
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
@@ -25,29 +25,26 @@ TODO:
 #ifndef BT_SLIDER_CONSTRAINT_H
 #define BT_SLIDER_CONSTRAINT_H
 
+#include "LinearMath/btScalar.h"  //for BT_USE_DOUBLE_PRECISION
+
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btSliderConstraintData2		btSliderConstraintDoubleData
-#define btSliderConstraintDataName  "btSliderConstraintDoubleData"
+#define btSliderConstraintData2 btSliderConstraintDoubleData
+#define btSliderConstraintDataName "btSliderConstraintDoubleData"
 #else
-#define btSliderConstraintData2		btSliderConstraintData 
-#define btSliderConstraintDataName	"btSliderConstraintData"
-#endif //BT_USE_DOUBLE_PRECISION
+#define btSliderConstraintData2 btSliderConstraintData
+#define btSliderConstraintDataName "btSliderConstraintData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
 #include "LinearMath/btVector3.h"
 #include "btJacobianEntry.h"
 #include "btTypedConstraint.h"
 
-
-
 class btRigidBody;
 
-
-
-#define SLIDER_CONSTRAINT_DEF_SOFTNESS		(btScalar(1.0))
-#define SLIDER_CONSTRAINT_DEF_DAMPING		(btScalar(1.0))
-#define SLIDER_CONSTRAINT_DEF_RESTITUTION	(btScalar(0.7))
-#define SLIDER_CONSTRAINT_DEF_CFM			(btScalar(0.f))
-
+#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0))
+#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0))
+#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7))
+#define SLIDER_CONSTRAINT_DEF_CFM (btScalar(0.f))
 
 enum btSliderFlags
 {
@@ -65,15 +62,15 @@ enum btSliderFlags
 	BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11)
 };
 
-
-ATTRIBUTE_ALIGNED16(class) btSliderConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class)
+btSliderConstraint : public btTypedConstraint
 {
 protected:
 	///for backwards compatibility during the transition to 'getInfo/getInfo2'
-	bool		m_useSolveConstraintObsolete;
-	bool		m_useOffsetForConstraintFrame;
-	btTransform	m_frameInA;
-    btTransform	m_frameInB;
+	bool m_useSolveConstraintObsolete;
+	bool m_useOffsetForConstraintFrame;
+	btTransform m_frameInA;
+	btTransform m_frameInB;
 	// use frameA fo define limits, if true
 	bool m_useLinearReferenceFrameA;
 	// linear limits
@@ -117,21 +114,21 @@ protected:
 	btScalar m_restitutionOrthoAng;
 	btScalar m_dampingOrthoAng;
 	btScalar m_cfmOrthoAng;
-	
+
 	// for interlal use
 	bool m_solveLinLim;
 	bool m_solveAngLim;
 
 	int m_flags;
 
-	btJacobianEntry	m_jacLin[3];
-	btScalar		m_jacLinDiagABInv[3];
+	btJacobianEntry m_jacLin[3];
+	btScalar m_jacLinDiagABInv[3];
 
-    btJacobianEntry	m_jacAng[3];
+	btJacobianEntry m_jacAng[3];
 
 	btScalar m_timeStep;
-    btTransform m_calculatedTransformA;
-    btTransform m_calculatedTransformB;
+	btTransform m_calculatedTransformA;
+	btTransform m_calculatedTransformB;
 
 	btVector3 m_sliderAxis;
 	btVector3 m_realPivotAInW;
@@ -148,57 +145,57 @@ protected:
 	btScalar m_angDepth;
 	btScalar m_kAngle;
 
-	bool	 m_poweredLinMotor;
-    btScalar m_targetLinMotorVelocity;
-    btScalar m_maxLinMotorForce;
-    btScalar m_accumulatedLinMotorImpulse;
-	
-	bool	 m_poweredAngMotor;
-    btScalar m_targetAngMotorVelocity;
-    btScalar m_maxAngMotorForce;
-    btScalar m_accumulatedAngMotorImpulse;
-
-	//------------------------    
+	bool m_poweredLinMotor;
+	btScalar m_targetLinMotorVelocity;
+	btScalar m_maxLinMotorForce;
+	btScalar m_accumulatedLinMotorImpulse;
+
+	bool m_poweredAngMotor;
+	btScalar m_targetAngMotorVelocity;
+	btScalar m_maxAngMotorForce;
+	btScalar m_accumulatedAngMotorImpulse;
+
+	//------------------------
 	void initParams();
+
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
+
 	// constructors
-    btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
-    btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
+	btSliderConstraint(btRigidBody & rbA, btRigidBody & rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA);
+	btSliderConstraint(btRigidBody & rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
 
 	// overrides
 
-    virtual void getInfo1 (btConstraintInfo1* info);
+	virtual void getInfo1(btConstraintInfo1 * info);
 
-	void getInfo1NonVirtual(btConstraintInfo1* info);
-	
-	virtual void getInfo2 (btConstraintInfo2* info);
+	void getInfo1NonVirtual(btConstraintInfo1 * info);
 
-	void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass);
+	virtual void getInfo2(btConstraintInfo2 * info);
 
+	void getInfo2NonVirtual(btConstraintInfo2 * info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, btScalar rbAinvMass, btScalar rbBinvMass);
 
 	// access
-    const btRigidBody& getRigidBodyA() const { return m_rbA; }
-    const btRigidBody& getRigidBodyB() const { return m_rbB; }
-    const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
-    const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }
-    const btTransform & getFrameOffsetA() const { return m_frameInA; }
-    const btTransform & getFrameOffsetB() const { return m_frameInB; }
-    btTransform & getFrameOffsetA() { return m_frameInA; }
-    btTransform & getFrameOffsetB() { return m_frameInB; }
-    btScalar getLowerLinLimit() { return m_lowerLinLimit; }
-    void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; }
-    btScalar getUpperLinLimit() { return m_upperLinLimit; }
-    void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
-    btScalar getLowerAngLimit() { return m_lowerAngLimit; }
-    void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
-    btScalar getUpperAngLimit() { return m_upperAngLimit; }
-    void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
+	const btRigidBody& getRigidBodyA() const { return m_rbA; }
+	const btRigidBody& getRigidBodyB() const { return m_rbB; }
+	const btTransform& getCalculatedTransformA() const { return m_calculatedTransformA; }
+	const btTransform& getCalculatedTransformB() const { return m_calculatedTransformB; }
+	const btTransform& getFrameOffsetA() const { return m_frameInA; }
+	const btTransform& getFrameOffsetB() const { return m_frameInB; }
+	btTransform& getFrameOffsetA() { return m_frameInA; }
+	btTransform& getFrameOffsetB() { return m_frameInB; }
+	btScalar getLowerLinLimit() { return m_lowerLinLimit; }
+	void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; }
+	btScalar getUpperLinLimit() { return m_upperLinLimit; }
+	void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
+	btScalar getLowerAngLimit() { return m_lowerAngLimit; }
+	void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
+	btScalar getUpperAngLimit() { return m_upperAngLimit; }
+	void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
 	bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
 	btScalar getSoftnessDirLin() { return m_softnessDirLin; }
 	btScalar getRestitutionDirLin() { return m_restitutionDirLin; }
-	btScalar getDampingDirLin() { return m_dampingDirLin ; }
+	btScalar getDampingDirLin() { return m_dampingDirLin; }
 	btScalar getSoftnessDirAng() { return m_softnessDirAng; }
 	btScalar getRestitutionDirAng() { return m_restitutionDirAng; }
 	btScalar getDampingDirAng() { return m_dampingDirAng; }
@@ -247,8 +244,6 @@ public:
 
 	btScalar getLinearPos() const { return m_linPos; }
 	btScalar getAngularPos() const { return m_angPos; }
-	
-	
 
 	// access for ODE solver
 	bool getSolveLinLimit() { return m_solveLinLim; }
@@ -256,9 +251,9 @@ public:
 	bool getSolveAngLimit() { return m_solveAngLim; }
 	btScalar getAngDepth() { return m_angDepth; }
 	// shared code used by ODE solver
-	void	calculateTransforms(const btTransform& transA,const btTransform& transB);
-	void	testLinLimits();
-	void	testAngLimits();
+	void calculateTransforms(const btTransform& transA, const btTransform& transB);
+	void testLinLimits();
+	void testAngLimits();
 	// access for PE Solver
 	btVector3 getAncorInA();
 	btVector3 getAncorInB();
@@ -266,84 +261,75 @@ public:
 	bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
 	void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
 
-	void setFrames(const btTransform& frameA, const btTransform& frameB) 
-	{ 
-		m_frameInA=frameA; 
-		m_frameInB=frameB;
-		calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	void setFrames(const btTransform& frameA, const btTransform& frameB)
+	{
+		m_frameInA = frameA;
+		m_frameInB = frameB;
+		calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
 		buildJacobian();
-	} 
-
+	}
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 	///If no axis is provided, it uses the default axis for this constraint.
-	virtual	void	setParam(int num, btScalar value, int axis = -1);
+	virtual void setParam(int num, btScalar value, int axis = -1);
 	///return the local value of parameter
-	virtual	btScalar getParam(int num, int axis = -1) const;
-	
-	virtual	int getFlags() const
-    	{
+	virtual btScalar getParam(int num, int axis = -1) const;
+
+	virtual int getFlags() const
+	{
 		return m_flags;
 	}
 
-	virtual	int	calculateSerializeBufferSize() const;
+	virtual int calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
-
-
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
-
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 
-
 struct btSliderConstraintData
 {
-	btTypedConstraintData	m_typeConstraintData;
-	btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTypedConstraintData m_typeConstraintData;
+	btTransformFloatData m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 	btTransformFloatData m_rbBFrame;
-	
-	float	m_linearUpperLimit;
-	float	m_linearLowerLimit;
 
-	float	m_angularUpperLimit;
-	float	m_angularLowerLimit;
+	float m_linearUpperLimit;
+	float m_linearLowerLimit;
 
-	int	m_useLinearReferenceFrameA;
-	int m_useOffsetForConstraintFrame;
+	float m_angularUpperLimit;
+	float m_angularLowerLimit;
 
+	int m_useLinearReferenceFrameA;
+	int m_useOffsetForConstraintFrame;
 };
 
-
 struct btSliderConstraintDoubleData
 {
-	btTypedConstraintDoubleData	m_typeConstraintData;
-	btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTypedConstraintDoubleData m_typeConstraintData;
+	btTransformDoubleData m_rbAFrame;  // constraint axii. Assumes z is hinge axis.
 	btTransformDoubleData m_rbBFrame;
-	
-	double	m_linearUpperLimit;
-	double	m_linearLowerLimit;
 
-	double	m_angularUpperLimit;
-	double	m_angularLowerLimit;
+	double m_linearUpperLimit;
+	double m_linearLowerLimit;
 
-	int	m_useLinearReferenceFrameA;
-	int m_useOffsetForConstraintFrame;
+	double m_angularUpperLimit;
+	double m_angularLowerLimit;
 
+	int m_useLinearReferenceFrameA;
+	int m_useOffsetForConstraintFrame;
 };
 
-SIMD_FORCE_INLINE		int	btSliderConstraint::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btSliderConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(btSliderConstraintData2);
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-SIMD_FORCE_INLINE	const char*	btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-
-	btSliderConstraintData2* sliderData = (btSliderConstraintData2*) dataBuffer;
-	btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer);
+	btSliderConstraintData2* sliderData = (btSliderConstraintData2*)dataBuffer;
+	btTypedConstraint::serialize(&sliderData->m_typeConstraintData, serializer);
 
 	m_frameInA.serialize(sliderData->m_rbAFrame);
 	m_frameInB.serialize(sliderData->m_rbBFrame);
@@ -360,7 +346,4 @@ SIMD_FORCE_INLINE	const char*	btSliderConstraint::serialize(void* dataBuffer, bt
 	return btSliderConstraintDataName;
 }
 
-
-
-#endif //BT_SLIDER_CONSTRAINT_H
-
+#endif  //BT_SLIDER_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
index 0c7dbd668bb..1ea20edcb2b 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
@@ -13,43 +13,38 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #include "btSolve2LinearConstraint.h"
 
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btVector3.h"
 #include "btJacobianEntry.h"
 
-
 void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
-												   btRigidBody* body1,
-		btRigidBody* body2,
-
-						const btMatrix3x3& world2A,
-						const btMatrix3x3& world2B,
-						
-						const btVector3& invInertiaADiag,
-						const btScalar invMassA,
-						const btVector3& linvelA,const btVector3& angvelA,
-						const btVector3& rel_posA1,
-						const btVector3& invInertiaBDiag,
-						const btScalar invMassB,
-						const btVector3& linvelB,const btVector3& angvelB,
-						const btVector3& rel_posA2,
-
-					  btScalar depthA, const btVector3& normalA, 
-					  const btVector3& rel_posB1,const btVector3& rel_posB2,
-					  btScalar depthB, const btVector3& normalB, 
-					  btScalar& imp0,btScalar& imp1)
+	btRigidBody* body1,
+	btRigidBody* body2,
+
+	const btMatrix3x3& world2A,
+	const btMatrix3x3& world2B,
+
+	const btVector3& invInertiaADiag,
+	const btScalar invMassA,
+	const btVector3& linvelA, const btVector3& angvelA,
+	const btVector3& rel_posA1,
+	const btVector3& invInertiaBDiag,
+	const btScalar invMassB,
+	const btVector3& linvelB, const btVector3& angvelB,
+	const btVector3& rel_posA2,
+
+	btScalar depthA, const btVector3& normalA,
+	const btVector3& rel_posB1, const btVector3& rel_posB2,
+	btScalar depthB, const btVector3& normalB,
+	btScalar& imp0, btScalar& imp1)
 {
 	(void)linvelA;
 	(void)linvelB;
 	(void)angvelB;
 	(void)angvelA;
 
-
-
 	imp0 = btScalar(0.);
 	imp1 = btScalar(0.);
 
@@ -59,86 +54,76 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
 
 	btAssert(len < SIMD_EPSILON);
 
-
 	//this jacobian entry could be re-used for all iterations
-	btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
-		invInertiaBDiag,invMassB);
-	btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
-		invInertiaBDiag,invMassB);
-	
+	btJacobianEntry jacA(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
+						 invInertiaBDiag, invMassB);
+	btJacobianEntry jacB(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
+						 invInertiaBDiag, invMassB);
+
 	//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
 	//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
 
-	const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
-	const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
+	const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1) - body2->getVelocityInLocalPoint(rel_posA1));
+	const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1) - body2->getVelocityInLocalPoint(rel_posB1));
 
-//	btScalar penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv
+	//	btScalar penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv
 	btScalar massTerm = btScalar(1.) / (invMassA + invMassB);
 
-
 	// calculate rhs (or error) terms
-	const btScalar dv0 = depthA  * m_tau * massTerm - vel0 * m_damping;
-	const btScalar dv1 = depthB  * m_tau * massTerm - vel1 * m_damping;
-
+	const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
+	const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
 
 	// dC/dv * dv = -C
-	
+
 	// jacobian * impulse = -error
 	//
 
 	//impulse = jacobianInverse * -error
 
 	// inverting 2x2 symmetric system (offdiagonal are equal!)
-	// 
+	//
 
+	btScalar nonDiag = jacA.getNonDiagonal(jacB, invMassA, invMassB);
+	btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag);
 
-	btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
-	btScalar	invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
-	
 	//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
 	//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
 
 	imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
-	imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
+	imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * -nonDiag * invDet;
 
 	//[a b]								  [d -c]
 	//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
 
 	//[jA nD] * [imp0] = [dv0]
 	//[nD jB]   [imp1]   [dv1]
-
 }
 
-
-
 void btSolve2LinearConstraint::resolveBilateralPairConstraint(
-						btRigidBody* body1,
-						btRigidBody* body2,
-						const btMatrix3x3& world2A,
-						const btMatrix3x3& world2B,
-						
-						const btVector3& invInertiaADiag,
-						const btScalar invMassA,
-						const btVector3& linvelA,const btVector3& angvelA,
-						const btVector3& rel_posA1,
-						const btVector3& invInertiaBDiag,
-						const btScalar invMassB,
-						const btVector3& linvelB,const btVector3& angvelB,
-						const btVector3& rel_posA2,
-
-					  btScalar depthA, const btVector3& normalA, 
-					  const btVector3& rel_posB1,const btVector3& rel_posB2,
-					  btScalar depthB, const btVector3& normalB, 
-					  btScalar& imp0,btScalar& imp1)
+	btRigidBody* body1,
+	btRigidBody* body2,
+	const btMatrix3x3& world2A,
+	const btMatrix3x3& world2B,
+
+	const btVector3& invInertiaADiag,
+	const btScalar invMassA,
+	const btVector3& linvelA, const btVector3& angvelA,
+	const btVector3& rel_posA1,
+	const btVector3& invInertiaBDiag,
+	const btScalar invMassB,
+	const btVector3& linvelB, const btVector3& angvelB,
+	const btVector3& rel_posA2,
+
+	btScalar depthA, const btVector3& normalA,
+	const btVector3& rel_posB1, const btVector3& rel_posB2,
+	btScalar depthB, const btVector3& normalB,
+	btScalar& imp0, btScalar& imp1)
 {
-
 	(void)linvelA;
 	(void)linvelB;
 	(void)angvelA;
 	(void)angvelB;
 
-
-
 	imp0 = btScalar(0.);
 	imp1 = btScalar(0.);
 
@@ -148,42 +133,40 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
 
 	btAssert(len < SIMD_EPSILON);
 
-
 	//this jacobian entry could be re-used for all iterations
-	btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
-		invInertiaBDiag,invMassB);
-	btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
-		invInertiaBDiag,invMassB);
-	
+	btJacobianEntry jacA(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
+						 invInertiaBDiag, invMassB);
+	btJacobianEntry jacB(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
+						 invInertiaBDiag, invMassB);
+
 	//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
 	//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
 
-	const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
-	const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
+	const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1) - body2->getVelocityInLocalPoint(rel_posA1));
+	const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1) - body2->getVelocityInLocalPoint(rel_posB1));
 
 	// calculate rhs (or error) terms
-	const btScalar dv0 = depthA  * m_tau - vel0 * m_damping;
-	const btScalar dv1 = depthB  * m_tau - vel1 * m_damping;
+	const btScalar dv0 = depthA * m_tau - vel0 * m_damping;
+	const btScalar dv1 = depthB * m_tau - vel1 * m_damping;
 
 	// dC/dv * dv = -C
-	
+
 	// jacobian * impulse = -error
 	//
 
 	//impulse = jacobianInverse * -error
 
 	// inverting 2x2 symmetric system (offdiagonal are equal!)
-	// 
+	//
 
+	btScalar nonDiag = jacA.getNonDiagonal(jacB, invMassA, invMassB);
+	btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag);
 
-	btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
-	btScalar	invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
-	
 	//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
 	//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
 
 	imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
-	imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
+	imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * -nonDiag * invDet;
 
 	//[a b]								  [d -c]
 	//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
@@ -191,9 +174,9 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
 	//[jA nD] * [imp0] = [dv0]
 	//[nD jB]   [imp1]   [dv1]
 
-	if ( imp0 > btScalar(0.0))
+	if (imp0 > btScalar(0.0))
 	{
-		if ( imp1 > btScalar(0.0) )
+		if (imp1 > btScalar(0.0))
 		{
 			//both positive
 		}
@@ -203,9 +186,10 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
 
 			// now imp0>0 imp1<0
 			imp0 = dv0 / jacA.getDiagonal();
-			if ( imp0 > btScalar(0.0) )
+			if (imp0 > btScalar(0.0))
 			{
-			} else
+			}
+			else
 			{
 				imp0 = btScalar(0.);
 			}
@@ -216,24 +200,25 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
 		imp0 = btScalar(0.);
 
 		imp1 = dv1 / jacB.getDiagonal();
-		if ( imp1 <= btScalar(0.0) )
+		if (imp1 <= btScalar(0.0))
 		{
 			imp1 = btScalar(0.);
 			// now imp0>0 imp1<0
 			imp0 = dv0 / jacA.getDiagonal();
-			if ( imp0 > btScalar(0.0) )
+			if (imp0 > btScalar(0.0))
 			{
-			} else
+			}
+			else
 			{
 				imp0 = btScalar(0.);
 			}
-		} else
+		}
+		else
 		{
 		}
 	}
 }
 
-
 /*
 void btSolve2LinearConstraint::resolveAngularConstraint(	const btMatrix3x3& invInertiaAWS,
 											const btScalar invMassA,
@@ -252,4 +237,3 @@ void btSolve2LinearConstraint::resolveAngularConstraint(	const btMatrix3x3& invI
 
 }
 */
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
index e8bfabf864e..fca8ecec81d 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
@@ -19,20 +19,16 @@ subject to the following restrictions:
 #include "LinearMath/btMatrix3x3.h"
 #include "LinearMath/btVector3.h"
 
-
 class btRigidBody;
 
-
-
 /// constraint class used for lateral tyre friction.
-class	btSolve2LinearConstraint
+class btSolve2LinearConstraint
 {
-	btScalar	m_tau;
-	btScalar	m_damping;
+	btScalar m_tau;
+	btScalar m_damping;
 
 public:
-
-	btSolve2LinearConstraint(btScalar tau,btScalar damping)
+	btSolve2LinearConstraint(btScalar tau, btScalar damping)
 	{
 		m_tau = tau;
 		m_damping = damping;
@@ -40,52 +36,51 @@ public:
 	//
 	// solve unilateral constraint (equality, direct method)
 	//
-	void resolveUnilateralPairConstraint(		
-														   btRigidBody* body0,
+	void resolveUnilateralPairConstraint(
+		btRigidBody* body0,
 		btRigidBody* body1,
 
 		const btMatrix3x3& world2A,
-						const btMatrix3x3& world2B,
-						
-						const btVector3& invInertiaADiag,
-						const btScalar invMassA,
-						const btVector3& linvelA,const btVector3& angvelA,
-						const btVector3& rel_posA1,
-						const btVector3& invInertiaBDiag,
-						const btScalar invMassB,
-						const btVector3& linvelB,const btVector3& angvelB,
-						const btVector3& rel_posA2,
-
-					  btScalar depthA, const btVector3& normalA, 
-					  const btVector3& rel_posB1,const btVector3& rel_posB2,
-					  btScalar depthB, const btVector3& normalB, 
-					  btScalar& imp0,btScalar& imp1);
-
+		const btMatrix3x3& world2B,
+
+		const btVector3& invInertiaADiag,
+		const btScalar invMassA,
+		const btVector3& linvelA, const btVector3& angvelA,
+		const btVector3& rel_posA1,
+		const btVector3& invInertiaBDiag,
+		const btScalar invMassB,
+		const btVector3& linvelB, const btVector3& angvelB,
+		const btVector3& rel_posA2,
+
+		btScalar depthA, const btVector3& normalA,
+		const btVector3& rel_posB1, const btVector3& rel_posB2,
+		btScalar depthB, const btVector3& normalB,
+		btScalar& imp0, btScalar& imp1);
 
 	//
 	// solving 2x2 lcp problem (inequality, direct solution )
 	//
 	void resolveBilateralPairConstraint(
-			btRigidBody* body0,
-						btRigidBody* body1,
+		btRigidBody* body0,
+		btRigidBody* body1,
 		const btMatrix3x3& world2A,
-						const btMatrix3x3& world2B,
-						
-						const btVector3& invInertiaADiag,
-						const btScalar invMassA,
-						const btVector3& linvelA,const btVector3& angvelA,
-						const btVector3& rel_posA1,
-						const btVector3& invInertiaBDiag,
-						const btScalar invMassB,
-						const btVector3& linvelB,const btVector3& angvelB,
-						const btVector3& rel_posA2,
-
-					  btScalar depthA, const btVector3& normalA, 
-					  const btVector3& rel_posB1,const btVector3& rel_posB2,
-					  btScalar depthB, const btVector3& normalB, 
-					  btScalar& imp0,btScalar& imp1);
-
-/*
+		const btMatrix3x3& world2B,
+
+		const btVector3& invInertiaADiag,
+		const btScalar invMassA,
+		const btVector3& linvelA, const btVector3& angvelA,
+		const btVector3& rel_posA1,
+		const btVector3& invInertiaBDiag,
+		const btScalar invMassB,
+		const btVector3& linvelB, const btVector3& angvelB,
+		const btVector3& rel_posA2,
+
+		btScalar depthA, const btVector3& normalA,
+		const btVector3& rel_posB1, const btVector3& rel_posB2,
+		btScalar depthB, const btVector3& normalB,
+		btScalar& imp0, btScalar& imp1);
+
+	/*
 	void resolveAngularConstraint(	const btMatrix3x3& invInertiaAWS,
 						const btScalar invMassA,
 						const btVector3& linvelA,const btVector3& angvelA,
@@ -101,7 +96,6 @@ public:
 					  btScalar& imp0,btScalar& imp1);
 
 */
-
 };
 
-#endif //BT_SOLVE_2LINEAR_CONSTRAINT_H
+#endif  //BT_SOLVE_2LINEAR_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h
index 8e4456e617a..409aa8a08cb 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h
@@ -16,7 +16,7 @@ subject to the following restrictions:
 #ifndef BT_SOLVER_BODY_H
 #define BT_SOLVER_BODY_H
 
-class	btRigidBody;
+class btRigidBody;
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btMatrix3x3.h"
 
@@ -26,108 +26,99 @@ class	btRigidBody;
 ///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
 #ifdef BT_USE_SSE
 #define USE_SIMD 1
-#endif //
-
+#endif  //
 
 #ifdef USE_SIMD
 
-struct	btSimdScalar
+struct btSimdScalar
 {
-	SIMD_FORCE_INLINE	btSimdScalar()
+	SIMD_FORCE_INLINE btSimdScalar()
 	{
-
 	}
-/* workaround for clang 3.4 ( == apple clang 5.1 ) issue, friction would fail with forced inlining */
-#if (defined(__clang__) && defined(__apple_build_version__) &&  (__clang_major__ == 5) && (__clang_minor__ == 1)) \
-|| (defined(__clang__) && !defined(__apple_build_version__) && (__clang_major__ == 3) && (__clang_minor__ == 4))
-	inline __attribute__ ((noinline)) btSimdScalar(float	fl)
-#else
-	SIMD_FORCE_INLINE	btSimdScalar(float	fl)
-#endif
-	:m_vec128 (_mm_set1_ps(fl))
+
+	SIMD_FORCE_INLINE btSimdScalar(float fl)
+		: m_vec128(_mm_set1_ps(fl))
 	{
 	}
 
-	SIMD_FORCE_INLINE	btSimdScalar(__m128 v128)
-		:m_vec128(v128)
+	SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
+		: m_vec128(v128)
 	{
 	}
-	union
-	{
-		__m128		m_vec128;
-		float		m_floats[4];
-		int			m_ints[4];
-		btScalar	m_unusedPadding;
+	union {
+		__m128 m_vec128;
+		float m_floats[4];
+		int m_ints[4];
+		btScalar m_unusedPadding;
 	};
-	SIMD_FORCE_INLINE	__m128	get128()
+	SIMD_FORCE_INLINE __m128 get128()
 	{
 		return m_vec128;
 	}
 
-	SIMD_FORCE_INLINE	const __m128	get128() const
+	SIMD_FORCE_INLINE const __m128 get128() const
 	{
 		return m_vec128;
 	}
 
-	SIMD_FORCE_INLINE	void	set128(__m128 v128)
+	SIMD_FORCE_INLINE void set128(__m128 v128)
 	{
 		m_vec128 = v128;
 	}
 
-	SIMD_FORCE_INLINE	operator       __m128()       
-	{ 
-		return m_vec128; 
-	}
-	SIMD_FORCE_INLINE	operator const __m128() const 
-	{ 
-		return m_vec128; 
+	SIMD_FORCE_INLINE operator __m128()
+	{
+		return m_vec128;
 	}
-	
-	SIMD_FORCE_INLINE	operator float() const 
-	{ 
-		return m_floats[0]; 
+	SIMD_FORCE_INLINE operator const __m128() const
+	{
+		return m_vec128;
 	}
 
+	SIMD_FORCE_INLINE operator float() const
+	{
+		return m_floats[0];
+	}
 };
 
 ///@brief Return the elementwise product of two btSimdScalar
-SIMD_FORCE_INLINE btSimdScalar 
-operator*(const btSimdScalar& v1, const btSimdScalar& v2) 
+SIMD_FORCE_INLINE btSimdScalar
+operator*(const btSimdScalar& v1, const btSimdScalar& v2)
 {
-	return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
+	return btSimdScalar(_mm_mul_ps(v1.get128(), v2.get128()));
 }
 
 ///@brief Return the elementwise product of two btSimdScalar
-SIMD_FORCE_INLINE btSimdScalar 
-operator+(const btSimdScalar& v1, const btSimdScalar& v2) 
+SIMD_FORCE_INLINE btSimdScalar
+operator+(const btSimdScalar& v1, const btSimdScalar& v2)
 {
-	return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
+	return btSimdScalar(_mm_add_ps(v1.get128(), v2.get128()));
 }
 
-
 #else
 #define btSimdScalar btScalar
 #endif
 
 ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
-ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
+ATTRIBUTE_ALIGNED16(struct)
+btSolverBody
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	btTransform		m_worldTransform;
-	btVector3		m_deltaLinearVelocity;
-	btVector3		m_deltaAngularVelocity;
-	btVector3		m_angularFactor;
-	btVector3		m_linearFactor;
-	btVector3		m_invMass;
-	btVector3		m_pushVelocity;
-	btVector3		m_turnVelocity;
-	btVector3		m_linearVelocity;
-	btVector3		m_angularVelocity;
-	btVector3		m_externalForceImpulse;
-	btVector3		m_externalTorqueImpulse;
-
-	btRigidBody*	m_originalBody;
-	void	setWorldTransform(const btTransform& worldTransform)
+	btTransform m_worldTransform;
+	btVector3 m_deltaLinearVelocity;
+	btVector3 m_deltaAngularVelocity;
+	btVector3 m_angularFactor;
+	btVector3 m_linearFactor;
+	btVector3 m_invMass;
+	btVector3 m_pushVelocity;
+	btVector3 m_turnVelocity;
+	btVector3 m_linearVelocity;
+	btVector3 m_angularVelocity;
+	btVector3 m_externalForceImpulse;
+	btVector3 m_externalTorqueImpulse;
+
+	btRigidBody* m_originalBody;
+	void setWorldTransform(const btTransform& worldTransform)
 	{
 		m_worldTransform = worldTransform;
 	}
@@ -136,56 +127,50 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 	{
 		return m_worldTransform;
 	}
-	
-	
 
-	SIMD_FORCE_INLINE void	getVelocityInLocalPointNoDelta(const btVector3& rel_pos, btVector3& velocity ) const
+	SIMD_FORCE_INLINE void getVelocityInLocalPointNoDelta(const btVector3& rel_pos, btVector3& velocity) const
 	{
 		if (m_originalBody)
-			velocity = m_linearVelocity + m_externalForceImpulse + (m_angularVelocity+m_externalTorqueImpulse).cross(rel_pos);
+			velocity = m_linearVelocity + m_externalForceImpulse + (m_angularVelocity + m_externalTorqueImpulse).cross(rel_pos);
 		else
-			velocity.setValue(0,0,0);
+			velocity.setValue(0, 0, 0);
 	}
 
-
-	SIMD_FORCE_INLINE void	getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
+	SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity) const
 	{
 		if (m_originalBody)
-			velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
+			velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
 		else
-			velocity.setValue(0,0,0);
+			velocity.setValue(0, 0, 0);
 	}
 
-	SIMD_FORCE_INLINE void	getAngularVelocity(btVector3& angVel) const
+	SIMD_FORCE_INLINE void getAngularVelocity(btVector3 & angVel) const
 	{
 		if (m_originalBody)
-			angVel =m_angularVelocity+m_deltaAngularVelocity;
+			angVel = m_angularVelocity + m_deltaAngularVelocity;
 		else
-			angVel.setValue(0,0,0);
+			angVel.setValue(0, 0, 0);
 	}
 
-
 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
+	SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent, const btScalar impulseMagnitude)
 	{
 		if (m_originalBody)
 		{
-			m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
-			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+			m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
+			m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
 		}
 	}
 
-	SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent, btScalar impulseMagnitude)
 	{
 		if (m_originalBody)
 		{
-			m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor;
-			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+			m_pushVelocity += linearComponent * impulseMagnitude * m_linearFactor;
+			m_turnVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
 		}
 	}
 
-
-
 	const btVector3& getDeltaLinearVelocity() const
 	{
 		return m_deltaLinearVelocity;
@@ -196,20 +181,19 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 		return m_deltaAngularVelocity;
 	}
 
-	const btVector3& getPushVelocity() const 
+	const btVector3& getPushVelocity() const
 	{
 		return m_pushVelocity;
 	}
 
-	const btVector3& getTurnVelocity() const 
+	const btVector3& getTurnVelocity() const
 	{
 		return m_turnVelocity;
 	}
 
-
 	////////////////////////////////////////////////
 	///some internal methods, don't use them
-		
+
 	btVector3& internalGetDeltaLinearVelocity()
 	{
 		return m_deltaLinearVelocity;
@@ -234,7 +218,7 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 	{
 		m_invMass = invMass;
 	}
-	
+
 	btVector3& internalGetPushVelocity()
 	{
 		return m_pushVelocity;
@@ -245,67 +229,57 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 		return m_turnVelocity;
 	}
 
-	SIMD_FORCE_INLINE void	internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
+	SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity) const
 	{
-		velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
+		velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
 	}
 
-	SIMD_FORCE_INLINE void	internalGetAngularVelocity(btVector3& angVel) const
+	SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3 & angVel) const
 	{
-		angVel = m_angularVelocity+m_deltaAngularVelocity;
+		angVel = m_angularVelocity + m_deltaAngularVelocity;
 	}
 
-
 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
+	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent, const btScalar impulseMagnitude)
 	{
 		if (m_originalBody)
 		{
-			m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
-			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+			m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
+			m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
 		}
 	}
-		
-	
-	
 
-	void	writebackVelocity()
+	void writebackVelocity()
 	{
 		if (m_originalBody)
 		{
-			m_linearVelocity +=m_deltaLinearVelocity;
+			m_linearVelocity += m_deltaLinearVelocity;
 			m_angularVelocity += m_deltaAngularVelocity;
-			
+
 			//m_originalBody->setCompanionId(-1);
 		}
 	}
 
-
-	void	writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp)
+	void writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp)
 	{
-        (void) timeStep;
+		(void)timeStep;
 		if (m_originalBody)
 		{
 			m_linearVelocity += m_deltaLinearVelocity;
 			m_angularVelocity += m_deltaAngularVelocity;
-			
+
 			//correct the position/orientation based on push/turn recovery
 			btTransform newTransform;
-			if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0)
+			if (m_pushVelocity[0] != 0.f || m_pushVelocity[1] != 0 || m_pushVelocity[2] != 0 || m_turnVelocity[0] != 0.f || m_turnVelocity[1] != 0 || m_turnVelocity[2] != 0)
 			{
-			//	btQuaternion orn = m_worldTransform.getRotation();
-				btTransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
+				//	btQuaternion orn = m_worldTransform.getRotation();
+				btTransformUtil::integrateTransform(m_worldTransform, m_pushVelocity, m_turnVelocity * splitImpulseTurnErp, timeStep, newTransform);
 				m_worldTransform = newTransform;
 			}
 			//m_worldTransform.setRotation(orn);
 			//m_originalBody->setCompanionId(-1);
 		}
 	}
-	
-
-
 };
 
-#endif //BT_SOLVER_BODY_H
-
-
+#endif  //BT_SOLVER_BODY_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
index 5515e6b311c..c7938df8675 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
@@ -16,7 +16,7 @@ subject to the following restrictions:
 #ifndef BT_SOLVER_CONSTRAINT_H
 #define BT_SOLVER_CONSTRAINT_H
 
-class	btRigidBody;
+class btRigidBody;
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btMatrix3x3.h"
 #include "btJacobianEntry.h"
@@ -25,56 +25,50 @@ class	btRigidBody;
 //#define NO_FRICTION_TANGENTIALS 1
 #include "btSolverBody.h"
 
-
 ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
-ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
+ATTRIBUTE_ALIGNED16(struct)
+btSolverConstraint
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btVector3		m_relpos1CrossNormal;
-	btVector3		m_contactNormal1;
-
-	btVector3		m_relpos2CrossNormal;
-	btVector3		m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
-
-	btVector3		m_angularComponentA;
-	btVector3		m_angularComponentB;
-	
-	mutable btSimdScalar	m_appliedPushImpulse;
-	mutable btSimdScalar	m_appliedImpulse;
-
-	btScalar	m_friction;
-	btScalar	m_jacDiagABInv;
-	btScalar		m_rhs;
-	btScalar		m_cfm;
-	
-    btScalar		m_lowerLimit;
-	btScalar		m_upperLimit;
-	btScalar		m_rhsPenetration;
-    union
-	{
-		void*		m_originalContactPoint;
-		btScalar	m_unusedPadding4;
-		int			m_numRowsForNonContactConstraint;
+	btVector3 m_relpos1CrossNormal;
+	btVector3 m_contactNormal1;
+
+	btVector3 m_relpos2CrossNormal;
+	btVector3 m_contactNormal2;  //usually m_contactNormal2 == -m_contactNormal1, but not always
+
+	btVector3 m_angularComponentA;
+	btVector3 m_angularComponentB;
+
+	mutable btSimdScalar m_appliedPushImpulse;
+	mutable btSimdScalar m_appliedImpulse;
+
+	btScalar m_friction;
+	btScalar m_jacDiagABInv;
+	btScalar m_rhs;
+	btScalar m_cfm;
+
+	btScalar m_lowerLimit;
+	btScalar m_upperLimit;
+	btScalar m_rhsPenetration;
+	union {
+		void* m_originalContactPoint;
+		btScalar m_unusedPadding4;
+		int m_numRowsForNonContactConstraint;
 	};
 
-	int	m_overrideNumSolverIterations;
-    int			m_frictionIndex;
+	int m_overrideNumSolverIterations;
+	int m_frictionIndex;
 	int m_solverBodyIdA;
 	int m_solverBodyIdB;
 
-    
-	enum		btSolverConstraintType
+	enum btSolverConstraintType
 	{
 		BT_SOLVER_CONTACT_1D = 0,
 		BT_SOLVER_FRICTION_1D
 	};
 };
 
-typedef btAlignedObjectArray<btSolverConstraint>	btConstraintArray;
-
-
-#endif //BT_SOLVER_CONSTRAINT_H
-
-
+typedef btAlignedObjectArray<btSolverConstraint> btConstraintArray;
 
+#endif  //BT_SOLVER_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
index 736a64a1c94..ebe679c449d 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
@@ -13,69 +13,63 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btTypedConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btSerializer.h"
 
-
-#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
+#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.05f)
 
 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
-:btTypedObject(type),
-m_userConstraintType(-1),
-m_userConstraintPtr((void*)-1),
-m_breakingImpulseThreshold(SIMD_INFINITY),
-m_isEnabled(true),
-m_needsFeedback(false),
-m_overrideNumSolverIterations(-1),
-m_rbA(rbA),
-m_rbB(getFixedBody()),
-m_appliedImpulse(btScalar(0.)),
-m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
-m_jointFeedback(0)
+	: btTypedObject(type),
+	  m_userConstraintType(-1),
+	  m_userConstraintPtr((void*)-1),
+	  m_breakingImpulseThreshold(SIMD_INFINITY),
+	  m_isEnabled(true),
+	  m_needsFeedback(false),
+	  m_overrideNumSolverIterations(-1),
+	  m_rbA(rbA),
+	  m_rbB(getFixedBody()),
+	  m_appliedImpulse(btScalar(0.)),
+	  m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
+	  m_jointFeedback(0)
 {
 }
 
-
-btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
-:btTypedObject(type),
-m_userConstraintType(-1),
-m_userConstraintPtr((void*)-1),
-m_breakingImpulseThreshold(SIMD_INFINITY),
-m_isEnabled(true),
-m_needsFeedback(false),
-m_overrideNumSolverIterations(-1),
-m_rbA(rbA),
-m_rbB(rbB),
-m_appliedImpulse(btScalar(0.)),
-m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
-m_jointFeedback(0)
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA, btRigidBody& rbB)
+	: btTypedObject(type),
+	  m_userConstraintType(-1),
+	  m_userConstraintPtr((void*)-1),
+	  m_breakingImpulseThreshold(SIMD_INFINITY),
+	  m_isEnabled(true),
+	  m_needsFeedback(false),
+	  m_overrideNumSolverIterations(-1),
+	  m_rbA(rbA),
+	  m_rbB(rbB),
+	  m_appliedImpulse(btScalar(0.)),
+	  m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
+	  m_jointFeedback(0)
 {
 }
 
-
-
-
 btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
 {
-	if(lowLim > uppLim)
+	if (lowLim > uppLim)
 	{
 		return btScalar(1.0f);
 	}
-	else if(lowLim == uppLim)
+	else if (lowLim == uppLim)
 	{
 		return btScalar(0.0f);
 	}
 	btScalar lim_fact = btScalar(1.0f);
 	btScalar delta_max = vel / timeFact;
-	if(delta_max < btScalar(0.0f))
+	if (delta_max < btScalar(0.0f))
 	{
-		if((pos >= lowLim) && (pos < (lowLim - delta_max)))
+		if ((pos >= lowLim) && (pos < (lowLim - delta_max)))
 		{
 			lim_fact = (lowLim - pos) / delta_max;
 		}
-		else if(pos  < lowLim)
+		else if (pos < lowLim)
 		{
 			lim_fact = btScalar(0.0f);
 		}
@@ -84,13 +78,13 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal
 			lim_fact = btScalar(1.0f);
 		}
 	}
-	else if(delta_max > btScalar(0.0f))
+	else if (delta_max > btScalar(0.0f))
 	{
-		if((pos <= uppLim) && (pos > (uppLim - delta_max)))
+		if ((pos <= uppLim) && (pos > (uppLim - delta_max)))
 		{
 			lim_fact = (uppLim - pos) / delta_max;
 		}
-		else if(pos  > uppLim)
+		else if (pos > uppLim)
 		{
 			lim_fact = btScalar(0.0f);
 		}
@@ -101,19 +95,19 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal
 	}
 	else
 	{
-			lim_fact = btScalar(0.0f);
+		lim_fact = btScalar(0.0f);
 	}
 	return lim_fact;
 }
 
 ///fills the dataBuffer and returns the struct name (and 0 on failure)
-const char*	btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
 {
-	btTypedConstraintData2* tcd = (btTypedConstraintData2*) dataBuffer;
+	btTypedConstraintData2* tcd = (btTypedConstraintData2*)dataBuffer;
 
 	tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
 	tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
-	char* name = (char*) serializer->findNameForPointer(this);
+	char* name = (char*)serializer->findNameForPointer(this);
 	tcd->m_name = (char*)serializer->getUniquePointer(name);
 	if (tcd->m_name)
 	{
@@ -124,10 +118,10 @@ const char*	btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
 	tcd->m_needsFeedback = m_needsFeedback;
 	tcd->m_overrideNumSolverIterations = m_overrideNumSolverIterations;
 	tcd->m_breakingImpulseThreshold = m_breakingImpulseThreshold;
-	tcd->m_isEnabled = m_isEnabled? 1: 0;
-	
-	tcd->m_userConstraintId =m_userConstraintId;
-	tcd->m_userConstraintType =m_userConstraintType;
+	tcd->m_isEnabled = m_isEnabled ? 1 : 0;
+
+	tcd->m_userConstraintId = m_userConstraintId;
+	tcd->m_userConstraintType = m_userConstraintType;
 
 	tcd->m_appliedImpulse = m_appliedImpulse;
 	tcd->m_dbgDrawSize = m_dbgDrawSize;
@@ -135,10 +129,10 @@ const char*	btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
 	tcd->m_disableCollisionsBetweenLinkedBodies = false;
 
 	int i;
-	for (i=0;i<m_rbA.getNumConstraintRefs();i++)
+	for (i = 0; i < m_rbA.getNumConstraintRefs(); i++)
 		if (m_rbA.getConstraintRef(i) == this)
 			tcd->m_disableCollisionsBetweenLinkedBodies = true;
-	for (i=0;i<m_rbB.getNumConstraintRefs();i++)
+	for (i = 0; i < m_rbB.getNumConstraintRefs(); i++)
 		if (m_rbB.getConstraintRef(i) == this)
 			tcd->m_disableCollisionsBetweenLinkedBodies = true;
 
@@ -147,17 +141,16 @@ const char*	btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
 
 btRigidBody& btTypedConstraint::getFixedBody()
 {
-	static btRigidBody s_fixed(0, 0,0);
-	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+	static btRigidBody s_fixed(0, 0, 0);
+	s_fixed.setMassProps(btScalar(0.), btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
 	return s_fixed;
 }
 
-
 void btAngularLimit::set(btScalar low, btScalar high, btScalar _softness, btScalar _biasFactor, btScalar _relaxationFactor)
 {
 	m_halfRange = (high - low) / 2.0f;
 	m_center = btNormalizeAngle(low + m_halfRange);
-	m_softness =  _softness;
+	m_softness = _softness;
 	m_biasFactor = _biasFactor;
 	m_relaxationFactor = _relaxationFactor;
 }
@@ -174,7 +167,7 @@ void btAngularLimit::test(const btScalar angle)
 		if (deviation < -m_halfRange)
 		{
 			m_solveLimit = true;
-			m_correction = - (deviation + m_halfRange);
+			m_correction = -(deviation + m_halfRange);
 			m_sign = +1.0f;
 		}
 		else if (deviation > m_halfRange)
@@ -186,7 +179,6 @@ void btAngularLimit::test(const btScalar angle)
 	}
 }
 
-
 btScalar btAngularLimit::getError() const
 {
 	return m_correction * m_sign;
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
index b55db0c1bfb..d30f3dee5c5 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
@@ -16,26 +16,24 @@ subject to the following restrictions:
 #ifndef BT_TYPED_CONSTRAINT_H
 #define BT_TYPED_CONSTRAINT_H
 
-
 #include "LinearMath/btScalar.h"
 #include "btSolverConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btTypedConstraintData2		btTypedConstraintDoubleData
-#define btTypedConstraintDataName	"btTypedConstraintDoubleData"
+#define btTypedConstraintData2 btTypedConstraintDoubleData
+#define btTypedConstraintDataName "btTypedConstraintDoubleData"
 #else
-#define btTypedConstraintData2 		btTypedConstraintFloatData
-#define btTypedConstraintDataName  "btTypedConstraintFloatData" 
-#endif //BT_USE_DOUBLE_PRECISION
-
+#define btTypedConstraintData2 btTypedConstraintFloatData
+#define btTypedConstraintDataName "btTypedConstraintFloatData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
 class btSerializer;
 
 //Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
 enum btTypedConstraintType
 {
-	POINT2POINT_CONSTRAINT_TYPE=3,
+	POINT2POINT_CONSTRAINT_TYPE = 3,
 	HINGE_CONSTRAINT_TYPE,
 	CONETWIST_CONSTRAINT_TYPE,
 	D6_CONSTRAINT_TYPE,
@@ -48,91 +46,88 @@ enum btTypedConstraintType
 	MAX_CONSTRAINT_TYPE
 };
 
-
 enum btConstraintParams
 {
-	BT_CONSTRAINT_ERP=1,
+	BT_CONSTRAINT_ERP = 1,
 	BT_CONSTRAINT_STOP_ERP,
 	BT_CONSTRAINT_CFM,
 	BT_CONSTRAINT_STOP_CFM
 };
 
 #if 1
-	#define btAssertConstrParams(_par) btAssert(_par) 
+#define btAssertConstrParams(_par) btAssert(_par)
 #else
-	#define btAssertConstrParams(_par)
+#define btAssertConstrParams(_par)
 #endif
 
-
-ATTRIBUTE_ALIGNED16(struct)	btJointFeedback
+ATTRIBUTE_ALIGNED16(struct)
+btJointFeedback
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	btVector3	m_appliedForceBodyA;
-	btVector3	m_appliedTorqueBodyA;
-	btVector3	m_appliedForceBodyB;
-	btVector3	m_appliedTorqueBodyB;
+	btVector3 m_appliedForceBodyA;
+	btVector3 m_appliedTorqueBodyA;
+	btVector3 m_appliedForceBodyB;
+	btVector3 m_appliedTorqueBodyB;
 };
 
-
 ///TypedConstraint is the baseclass for Bullet constraints and vehicles
-ATTRIBUTE_ALIGNED16(class) btTypedConstraint : public btTypedObject
+ATTRIBUTE_ALIGNED16(class)
+btTypedConstraint : public btTypedObject
 {
-	int	m_userConstraintType;
+	int m_userConstraintType;
 
-	union
-	{
-		int	m_userConstraintId;
+	union {
+		int m_userConstraintId;
 		void* m_userConstraintPtr;
 	};
 
-	btScalar	m_breakingImpulseThreshold;
-	bool		m_isEnabled;
-	bool		m_needsFeedback;
-	int			m_overrideNumSolverIterations;
+	btScalar m_breakingImpulseThreshold;
+	bool m_isEnabled;
+	bool m_needsFeedback;
+	int m_overrideNumSolverIterations;
 
-
-	btTypedConstraint&	operator=(btTypedConstraint&	other)
+	btTypedConstraint& operator=(btTypedConstraint& other)
 	{
 		btAssert(0);
-		(void) other;
+		(void)other;
 		return *this;
 	}
 
 protected:
-	btRigidBody&	m_rbA;
-	btRigidBody&	m_rbB;
-	btScalar	m_appliedImpulse;
-	btScalar	m_dbgDrawSize;
-	btJointFeedback*	m_jointFeedback;
+	btRigidBody& m_rbA;
+	btRigidBody& m_rbB;
+	btScalar m_appliedImpulse;
+	btScalar m_dbgDrawSize;
+	btJointFeedback* m_jointFeedback;
 
 	///internal method used by the constraint solver, don't use them directly
 	btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
-	
 
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	virtual ~btTypedConstraint() {};
-	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
-	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
+	virtual ~btTypedConstraint(){};
+	btTypedConstraint(btTypedConstraintType type, btRigidBody & rbA);
+	btTypedConstraint(btTypedConstraintType type, btRigidBody & rbA, btRigidBody & rbB);
 
-	struct btConstraintInfo1 {
-		int m_numConstraintRows,nub;
+	struct btConstraintInfo1
+	{
+		int m_numConstraintRows, nub;
 	};
 
 	static btRigidBody& getFixedBody();
 
-	struct btConstraintInfo2 {
+	struct btConstraintInfo2
+	{
 		// integrator parameters: frames per second (1/stepsize), default error
 		// reduction parameter (0..1).
-		btScalar fps,erp;
+		btScalar fps, erp;
 
 		// for the first and second body, pointers to two (linear and angular)
 		// n*3 jacobian sub matrices, stored by rows. these matrices will have
 		// been initialized to 0 on entry. if the second body is zero then the
 		// J2xx pointers may be 0.
-		btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
+		btScalar *m_J1linearAxis, *m_J1angularAxis, *m_J2linearAxis, *m_J2angularAxis;
 
 		// elements to jump from one row to the next in J's
 		int rowskip;
@@ -140,24 +135,19 @@ public:
 		// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
 		// "constraint force mixing" vector. c is set to zero on entry, cfm is
 		// set to a constant value (typically very small or zero) value on entry.
-		btScalar *m_constraintError,*cfm;
+		btScalar *m_constraintError, *cfm;
 
 		// lo and hi limits for variables (set to -/+ infinity on entry).
-		btScalar *m_lowerLimit,*m_upperLimit;
+		btScalar *m_lowerLimit, *m_upperLimit;
 
-		// findex vector for variables. see the LCP solver interface for a
-		// description of what this does. this is set to -1 on entry.
-		// note that the returned indexes are relative to the first index of
-		// the constraint.
-		int *findex;
 		// number of solver iterations
 		int m_numIterations;
 
 		//damping of the velocity
-		btScalar	m_damping;
+		btScalar m_damping;
 	};
 
-	int	getOverrideNumSolverIterations() const
+	int getOverrideNumSolverIterations() const
 	{
 		return m_overrideNumSolverIterations;
 	}
@@ -170,60 +160,57 @@ public:
 	}
 
 	///internal method used by the constraint solver, don't use them directly
-	virtual void	buildJacobian() {};
+	virtual void buildJacobian(){};
 
 	///internal method used by the constraint solver, don't use them directly
-	virtual	void	setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
+	virtual void setupSolverConstraint(btConstraintArray & ca, int solverBodyA, int solverBodyB, btScalar timeStep)
 	{
-        (void)ca;
-        (void)solverBodyA;
-        (void)solverBodyB;
-        (void)timeStep;
+		(void)ca;
+		(void)solverBodyA;
+		(void)solverBodyB;
+		(void)timeStep;
 	}
-	
+
 	///internal method used by the constraint solver, don't use them directly
-	virtual void getInfo1 (btConstraintInfo1* info)=0;
+	virtual void getInfo1(btConstraintInfo1 * info) = 0;
 
 	///internal method used by the constraint solver, don't use them directly
-	virtual void getInfo2 (btConstraintInfo2* info)=0;
+	virtual void getInfo2(btConstraintInfo2 * info) = 0;
 
 	///internal method used by the constraint solver, don't use them directly
-	void	internalSetAppliedImpulse(btScalar appliedImpulse)
+	void internalSetAppliedImpulse(btScalar appliedImpulse)
 	{
 		m_appliedImpulse = appliedImpulse;
 	}
 	///internal method used by the constraint solver, don't use them directly
-	btScalar	internalGetAppliedImpulse()
+	btScalar internalGetAppliedImpulse()
 	{
 		return m_appliedImpulse;
 	}
 
-
-	btScalar	getBreakingImpulseThreshold() const
+	btScalar getBreakingImpulseThreshold() const
 	{
-		return 	m_breakingImpulseThreshold;
+		return m_breakingImpulseThreshold;
 	}
 
-	void	setBreakingImpulseThreshold(btScalar threshold)
+	void setBreakingImpulseThreshold(btScalar threshold)
 	{
 		m_breakingImpulseThreshold = threshold;
 	}
 
-	bool	isEnabled() const
+	bool isEnabled() const
 	{
 		return m_isEnabled;
 	}
 
-	void	setEnabled(bool enabled)
+	void setEnabled(bool enabled)
 	{
-		m_isEnabled=enabled;
+		m_isEnabled = enabled;
 	}
 
-
 	///internal method used by the constraint solver, don't use them directly
-	virtual	void	solveConstraintObsolete(btSolverBody& /*bodyA*/,btSolverBody& /*bodyB*/,btScalar	/*timeStep*/) {};
+	virtual void solveConstraintObsolete(btSolverBody& /*bodyA*/, btSolverBody& /*bodyB*/, btScalar /*timeStep*/){};
 
-	
 	const btRigidBody& getRigidBodyA() const
 	{
 		return m_rbA;
@@ -233,7 +220,7 @@ public:
 		return m_rbB;
 	}
 
-	btRigidBody& getRigidBodyA() 
+	btRigidBody& getRigidBodyA()
 	{
 		return m_rbA;
 	}
@@ -244,15 +231,15 @@ public:
 
 	int getUserConstraintType() const
 	{
-		return m_userConstraintType ;
+		return m_userConstraintType;
 	}
 
-	void	setUserConstraintType(int userConstraintType)
+	void setUserConstraintType(int userConstraintType)
 	{
 		m_userConstraintType = userConstraintType;
 	};
 
-	void	setUserConstraintId(int uid)
+	void setUserConstraintId(int uid)
 	{
 		m_userConstraintId = uid;
 	}
@@ -262,17 +249,17 @@ public:
 		return m_userConstraintId;
 	}
 
-	void	setUserConstraintPtr(void* ptr)
+	void setUserConstraintPtr(void* ptr)
 	{
 		m_userConstraintPtr = ptr;
 	}
 
-	void*	getUserConstraintPtr()
+	void* getUserConstraintPtr()
 	{
 		return m_userConstraintPtr;
 	}
 
-	void	setJointFeedback(btJointFeedback* jointFeedback)
+	void setJointFeedback(btJointFeedback * jointFeedback)
 	{
 		m_jointFeedback = jointFeedback;
 	}
@@ -287,37 +274,36 @@ public:
 		return m_jointFeedback;
 	}
 
-
 	int getUid() const
 	{
-		return m_userConstraintId;   
-	} 
+		return m_userConstraintId;
+	}
 
-	bool	needsFeedback() const
+	bool needsFeedback() const
 	{
 		return m_needsFeedback;
 	}
 
 	///enableFeedback will allow to read the applied linear and angular impulse
 	///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
-	void	enableFeedback(bool needsFeedback)
+	void enableFeedback(bool needsFeedback)
 	{
 		m_needsFeedback = needsFeedback;
 	}
 
-	///getAppliedImpulse is an estimated total applied impulse. 
+	///getAppliedImpulse is an estimated total applied impulse.
 	///This feedback could be used to determine breaking constraints or playing sounds.
-	btScalar	getAppliedImpulse() const
+	btScalar getAppliedImpulse() const
 	{
 		btAssert(m_needsFeedback);
 		return m_appliedImpulse;
 	}
 
-	btTypedConstraintType getConstraintType () const
+	btTypedConstraintType getConstraintType() const
 	{
 		return btTypedConstraintType(m_objectType);
 	}
-	
+
 	void setDbgDrawSize(btScalar dbgDrawSize)
 	{
 		m_dbgDrawSize = dbgDrawSize;
@@ -327,35 +313,34 @@ public:
 		return m_dbgDrawSize;
 	}
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 	///If no axis is provided, it uses the default axis for this constraint.
-	virtual	void	setParam(int num, btScalar value, int axis = -1) = 0;
+	virtual void setParam(int num, btScalar value, int axis = -1) = 0;
 
 	///return the local value of parameter
-	virtual	btScalar getParam(int num, int axis = -1) const = 0;
-	
-	virtual	int	calculateSerializeBufferSize() const;
+	virtual btScalar getParam(int num, int axis = -1) const = 0;
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+	virtual int calculateSerializeBufferSize() const;
 
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
 };
 
-// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits 
+// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
 // all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
 SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
 {
-	if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
+	if (angleLowerLimitInRadians >= angleUpperLimitInRadians)
 	{
 		return angleInRadians;
 	}
-	else if(angleInRadians < angleLowerLimitInRadians)
+	else if (angleInRadians < angleLowerLimitInRadians)
 	{
 		btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
 		btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
 		return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
 	}
-	else if(angleInRadians > angleUpperLimitInRadians)
+	else if (angleInRadians > angleUpperLimitInRadians)
 	{
 		btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
 		btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
@@ -367,6 +352,8 @@ SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScal
 	}
 }
 
+// clang-format off
+
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	btTypedConstraintFloatData
 {
@@ -390,6 +377,8 @@ struct	btTypedConstraintFloatData
 	
 };
 
+
+
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 
 #define BT_BACKWARDS_COMPATIBLE_SERIALIZATION
@@ -441,18 +430,17 @@ struct	btTypedConstraintDoubleData
 	
 };
 
+// clang-format on
 
-SIMD_FORCE_INLINE	int	btTypedConstraint::calculateSerializeBufferSize() const
+SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(btTypedConstraintData2);
 }
 
-
-
 class btAngularLimit
 {
 private:
-	btScalar 
+	btScalar
 		m_center,
 		m_halfRange,
 		m_softness,
@@ -467,15 +455,16 @@ private:
 public:
 	/// Default constructor initializes limit as inactive, allowing free constraint movement
 	btAngularLimit()
-		:m_center(0.0f),
-		m_halfRange(-1.0f),
-		m_softness(0.9f),
-		m_biasFactor(0.3f),
-		m_relaxationFactor(1.0f),
-		m_correction(0.0f),
-		m_sign(0.0f),
-		m_solveLimit(false)
-	{}
+		: m_center(0.0f),
+		  m_halfRange(-1.0f),
+		  m_softness(0.9f),
+		  m_biasFactor(0.3f),
+		  m_relaxationFactor(1.0f),
+		  m_correction(0.0f),
+		  m_sign(0.0f),
+		  m_solveLimit(false)
+	{
+	}
 
 	/// Sets all limit's parameters.
 	/// When low > high limit becomes inactive.
@@ -504,13 +493,13 @@ public:
 		return m_relaxationFactor;
 	}
 
-	/// Returns correction value evaluated when test() was invoked 
+	/// Returns correction value evaluated when test() was invoked
 	inline btScalar getCorrection() const
 	{
 		return m_correction;
 	}
 
-	/// Returns sign value evaluated when test() was invoked 
+	/// Returns sign value evaluated when test() was invoked
 	inline btScalar getSign() const
 	{
 		return m_sign;
@@ -538,9 +527,6 @@ public:
 	btScalar getLow() const;
 
 	btScalar getHigh() const;
-
 };
 
-
-
-#endif //BT_TYPED_CONSTRAINT_H
+#endif  //BT_TYPED_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp
index b009f41aec8..42ed1fbb878 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp
@@ -13,43 +13,38 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #include "btUniversalConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
 
-
-
 #define UNIV_EPS btScalar(0.01f)
 
-
 // constructor
 // anchor, axis1 and axis2 are in world coordinate system
 // axis1 must be orthogonal to axis2
 btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2)
-: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
- m_anchor(anchor),
- m_axis1(axis1),
- m_axis2(axis2)
+	: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
+	  m_anchor(anchor),
+	  m_axis1(axis1),
+	  m_axis2(axis2)
 {
 	// build frame basis
 	// 6DOF constraint uses Euler angles and to define limits
 	// it is assumed that rotational order is :
 	// Z - first, allowed limits are (-PI,PI);
-	// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number 
+	// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
 	// used to prevent constraint from instability on poles;
 	// new position of X, allowed limits are (-PI,PI);
 	// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
 	// Build the frame in world coordinate system first
 	btVector3 zAxis = m_axis1.normalize();
 	btVector3 yAxis = m_axis2.normalize();
-	btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
+	btVector3 xAxis = yAxis.cross(zAxis);  // we want right coordinate system
 	btTransform frameInW;
 	frameInW.setIdentity();
-	frameInW.getBasis().setValue(	xAxis[0], yAxis[0], zAxis[0],	
-									xAxis[1], yAxis[1], zAxis[1],
-									xAxis[2], yAxis[2], zAxis[2]);
+	frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
+								 xAxis[1], yAxis[1], zAxis[1],
+								 xAxis[2], yAxis[2], zAxis[2]);
 	frameInW.setOrigin(anchor);
 	// now get constraint frame in local coordinate systems
 	m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
@@ -58,30 +53,28 @@ btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB,
 	setLinearLowerLimit(btVector3(0., 0., 0.));
 	setLinearUpperLimit(btVector3(0., 0., 0.));
 	setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS));
-	setAngularUpperLimit(btVector3(0.f,  SIMD_HALF_PI - UNIV_EPS,  SIMD_PI - UNIV_EPS));
+	setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS));
 }
 
-void btUniversalConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
+void btUniversalConstraint::setAxis(const btVector3& axis1, const btVector3& axis2)
 {
-  m_axis1 = axis1;
-  m_axis2 = axis2;
+	m_axis1 = axis1;
+	m_axis2 = axis2;
 
 	btVector3 zAxis = axis1.normalized();
 	btVector3 yAxis = axis2.normalized();
-	btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
+	btVector3 xAxis = yAxis.cross(zAxis);  // we want right coordinate system
 
 	btTransform frameInW;
 	frameInW.setIdentity();
-	frameInW.getBasis().setValue(	xAxis[0], yAxis[0], zAxis[0],	
-                                xAxis[1], yAxis[1], zAxis[1],
-                                xAxis[2], yAxis[2], zAxis[2]);
+	frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
+								 xAxis[1], yAxis[1], zAxis[1],
+								 xAxis[2], yAxis[2], zAxis[2]);
 	frameInW.setOrigin(m_anchor);
 
 	// now get constraint frame in local coordinate systems
 	m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
 	m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
 
-  calculateTransforms();
+	calculateTransforms();
 }
-
-
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h
index 9e708410430..8c24d93a641 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h
@@ -16,35 +16,32 @@ subject to the following restrictions:
 #ifndef BT_UNIVERSAL_CONSTRAINT_H
 #define BT_UNIVERSAL_CONSTRAINT_H
 
-
-
 #include "LinearMath/btVector3.h"
 #include "btTypedConstraint.h"
 #include "btGeneric6DofConstraint.h"
 
-
-
 /// Constraint similar to ODE Universal Joint
 /// has 2 rotatioonal degrees of freedom, similar to Euler rotations around Z (axis 1)
 /// and Y (axis 2)
-/// Description from ODE manual : 
-/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular. 
+/// Description from ODE manual :
+/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular.
 /// In other words, rotation of the two bodies about the direction perpendicular to the two axes will be equal."
 
-ATTRIBUTE_ALIGNED16(class) btUniversalConstraint : public btGeneric6DofConstraint
+ATTRIBUTE_ALIGNED16(class)
+btUniversalConstraint : public btGeneric6DofConstraint
 {
 protected:
-	btVector3	m_anchor;
-	btVector3	m_axis1;
-	btVector3	m_axis2;
+	btVector3 m_anchor;
+	btVector3 m_axis1;
+	btVector3 m_axis2;
+
 public:
-	
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
+
 	// constructor
 	// anchor, axis1 and axis2 are in world coordinate system
 	// axis1 must be orthogonal to axis2
-    btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2);
+	btUniversalConstraint(btRigidBody & rbA, btRigidBody & rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2);
 	// access
 	const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
 	const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
@@ -56,10 +53,7 @@ public:
 	void setUpperLimit(btScalar ang1max, btScalar ang2max) { setAngularUpperLimit(btVector3(0.f, ang1max, ang2max)); }
 	void setLowerLimit(btScalar ang1min, btScalar ang2min) { setAngularLowerLimit(btVector3(0.f, ang1min, ang2min)); }
 
-	void setAxis( const btVector3& axis1, const btVector3& axis2);
+	void setAxis(const btVector3& axis1, const btVector3& axis2);
 };
 
-
-
-#endif // BT_UNIVERSAL_CONSTRAINT_H
-
+#endif  // BT_UNIVERSAL_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp
deleted file mode 100644
index e1f69afe101..00000000000
--- a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp
+++ /dev/null
@@ -1,469 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-/*
-	Draft high-level generic physics C-API. For low-level access, use the physics SDK native API's.
-	Work in progress, functionality will be added on demand.
-
-	If possible, use the richer Bullet C++ API, by including <src/btBulletDynamicsCommon.h>
-*/
-
-#include "Bullet-C-Api.h"
-#include "btBulletDynamicsCommon.h"
-#include "LinearMath/btAlignedAllocator.h"
-#include "LinearMath/btConvexHullComputer.h"
-
-
-#include "LinearMath/btVector3.h"
-#include "LinearMath/btScalar.h"	
-#include "LinearMath/btMatrix3x3.h"
-#include "LinearMath/btTransform.h"
-#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
-#include "BulletCollision/CollisionShapes/btTriangleShape.h"
-#include "BulletCollision/Gimpact/btTriangleShapeEx.h"
-
-#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
-#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
-#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
-#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
-#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
-#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
-#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
-#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
-
-
-/*
-	Create and Delete a Physics SDK	
-*/
-
-struct	btPhysicsSdk
-{
-
-//	btDispatcher*				m_dispatcher;
-//	btOverlappingPairCache*		m_pairCache;
-//	btConstraintSolver*			m_constraintSolver
-
-	btVector3	m_worldAabbMin;
-	btVector3	m_worldAabbMax;
-
-
-	//todo: version, hardware/optimization settings etc?
-	btPhysicsSdk()
-		:m_worldAabbMin(-1000,-1000,-1000),
-		m_worldAabbMax(1000,1000,1000)
-	{
-
-	}
-
-	
-};
-
-plPhysicsSdkHandle	plNewBulletSdk()
-{
-	void* mem = btAlignedAlloc(sizeof(btPhysicsSdk),16);
-	return (plPhysicsSdkHandle)new (mem)btPhysicsSdk;
-}
-
-void		plDeletePhysicsSdk(plPhysicsSdkHandle	physicsSdk)
-{
-	btPhysicsSdk* phys = reinterpret_cast<btPhysicsSdk*>(physicsSdk);
-	btAlignedFree(phys);	
-}
-
-
-/* Dynamics World */
-plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdkHandle)
-{
-	btPhysicsSdk* physicsSdk = reinterpret_cast<btPhysicsSdk*>(physicsSdkHandle);
-	void* mem = btAlignedAlloc(sizeof(btDefaultCollisionConfiguration),16);
-	btDefaultCollisionConfiguration* collisionConfiguration = new (mem)btDefaultCollisionConfiguration();
-	mem = btAlignedAlloc(sizeof(btCollisionDispatcher),16);
-	btDispatcher*				dispatcher = new (mem)btCollisionDispatcher(collisionConfiguration);
-	mem = btAlignedAlloc(sizeof(btAxisSweep3),16);
-	btBroadphaseInterface*		pairCache = new (mem)btAxisSweep3(physicsSdk->m_worldAabbMin,physicsSdk->m_worldAabbMax);
-	mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
-	btConstraintSolver*			constraintSolver = new(mem) btSequentialImpulseConstraintSolver();
-
-	mem = btAlignedAlloc(sizeof(btDiscreteDynamicsWorld),16);
-	return (plDynamicsWorldHandle) new (mem)btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration);
-}
-void           plDeleteDynamicsWorld(plDynamicsWorldHandle world)
-{
-	//todo: also clean up the other allocations, axisSweep, pairCache,dispatcher,constraintSolver,collisionConfiguration
-	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
-	btAlignedFree(dynamicsWorld);
-}
-
-void	plStepSimulation(plDynamicsWorldHandle world,	plReal	timeStep)
-{
-	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
-	btAssert(dynamicsWorld);
-	dynamicsWorld->stepSimulation(timeStep);
-}
-
-void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
-{
-	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
-	btAssert(dynamicsWorld);
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-
-	dynamicsWorld->addRigidBody(body);
-}
-
-void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
-{
-	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
-	btAssert(dynamicsWorld);
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-
-	dynamicsWorld->removeRigidBody(body);
-}
-
-/* Rigid Body  */
-
-plRigidBodyHandle plCreateRigidBody(	void* user_data,  float mass, plCollisionShapeHandle cshape )
-{
-	btTransform trans;
-	trans.setIdentity();
-	btVector3 localInertia(0,0,0);
-	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
-	btAssert(shape);
-	if (mass)
-	{
-		shape->calculateLocalInertia(mass,localInertia);
-	}
-	void* mem = btAlignedAlloc(sizeof(btRigidBody),16);
-	btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0,shape,localInertia);
-	btRigidBody* body = new (mem)btRigidBody(rbci);
-	body->setWorldTransform(trans);
-	body->setUserPointer(user_data);
-	return (plRigidBodyHandle) body;
-}
-
-void plDeleteRigidBody(plRigidBodyHandle cbody)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody);
-	btAssert(body);
-	btAlignedFree( body);
-}
-
-
-/* Collision Shape definition */
-
-plCollisionShapeHandle plNewSphereShape(plReal radius)
-{
-	void* mem = btAlignedAlloc(sizeof(btSphereShape),16);
-	return (plCollisionShapeHandle) new (mem)btSphereShape(radius);
-	
-}
-	
-plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z)
-{
-	void* mem = btAlignedAlloc(sizeof(btBoxShape),16);
-	return (plCollisionShapeHandle) new (mem)btBoxShape(btVector3(x,y,z));
-}
-
-plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height)
-{
-	//capsule is convex hull of 2 spheres, so use btMultiSphereShape
-	
-	const int numSpheres = 2;
-	btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)};
-	btScalar radi[numSpheres] = {radius,radius};
-	void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16);
-	return (plCollisionShapeHandle) new (mem)btMultiSphereShape(positions,radi,numSpheres);
-}
-plCollisionShapeHandle plNewConeShape(plReal radius, plReal height)
-{
-	void* mem = btAlignedAlloc(sizeof(btConeShape),16);
-	return (plCollisionShapeHandle) new (mem)btConeShape(radius,height);
-}
-
-plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height)
-{
-	void* mem = btAlignedAlloc(sizeof(btCylinderShape),16);
-	return (plCollisionShapeHandle) new (mem)btCylinderShape(btVector3(radius,height,radius));
-}
-
-/* Convex Meshes */
-plCollisionShapeHandle plNewConvexHullShape()
-{
-	void* mem = btAlignedAlloc(sizeof(btConvexHullShape),16);
-	return (plCollisionShapeHandle) new (mem)btConvexHullShape();
-}
-
-
-/* Concave static triangle meshes */
-plMeshInterfaceHandle		   plNewMeshInterface()
-{
-	return 0;
-}
-
-plCollisionShapeHandle plNewCompoundShape()
-{
-	void* mem = btAlignedAlloc(sizeof(btCompoundShape),16);
-	return (plCollisionShapeHandle) new (mem)btCompoundShape();
-}
-
-void	plAddChildShape(plCollisionShapeHandle compoundShapeHandle,plCollisionShapeHandle childShapeHandle, plVector3 childPos,plQuaternion childOrn)
-{
-	btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>(compoundShapeHandle);
-	btAssert(colShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE);
-	btCompoundShape* compoundShape = reinterpret_cast<btCompoundShape*>(colShape);
-	btCollisionShape* childShape = reinterpret_cast<btCollisionShape*>(childShapeHandle);
-	btTransform	localTrans;
-	localTrans.setIdentity();
-	localTrans.setOrigin(btVector3(childPos[0],childPos[1],childPos[2]));
-	localTrans.setRotation(btQuaternion(childOrn[0],childOrn[1],childOrn[2],childOrn[3]));
-	compoundShape->addChildShape(localTrans,childShape);
-}
-
-void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient)
-{
-	btQuaternion orn;
-	orn.setEuler(yaw,pitch,roll);
-	orient[0] = orn.getX();
-	orient[1] = orn.getY();
-	orient[2] = orn.getZ();
-	orient[3] = orn.getW();
-
-}
-
-
-//	extern  void		plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2);
-//	extern  plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle);
-
-
-void		plAddVertex(plCollisionShapeHandle cshape, plReal x,plReal y,plReal z)
-{
-	btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>( cshape);
-	(void)colShape;
-	btAssert(colShape->getShapeType()==CONVEX_HULL_SHAPE_PROXYTYPE);
-	btConvexHullShape* convexHullShape = reinterpret_cast<btConvexHullShape*>( cshape);
-	convexHullShape->addPoint(btVector3(x,y,z));
-
-}
-
-void plDeleteShape(plCollisionShapeHandle cshape)
-{
-	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
-	btAssert(shape);
-	btAlignedFree(shape);
-}
-void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling)
-{
-	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
-	btAssert(shape);
-	btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]);
-	shape->setLocalScaling(scaling);	
-}
-
-
-
-void plSetPosition(plRigidBodyHandle object, const plVector3 position)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	btVector3 pos(position[0],position[1],position[2]);
-	btTransform worldTrans = body->getWorldTransform();
-	worldTrans.setOrigin(pos);
-	body->setWorldTransform(worldTrans);
-}
-
-void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	btQuaternion orn(orientation[0],orientation[1],orientation[2],orientation[3]);
-	btTransform worldTrans = body->getWorldTransform();
-	worldTrans.setRotation(orn);
-	body->setWorldTransform(worldTrans);
-}
-
-void	plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	btTransform& worldTrans = body->getWorldTransform();
-	worldTrans.setFromOpenGLMatrix(matrix);
-}
-
-void	plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	body->getWorldTransform().getOpenGLMatrix(matrix);
-
-}
-
-void	plGetPosition(plRigidBodyHandle object,plVector3 position)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	const btVector3& pos = body->getWorldTransform().getOrigin();
-	position[0] = pos.getX();
-	position[1] = pos.getY();
-	position[2] = pos.getZ();
-}
-
-void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation)
-{
-	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
-	btAssert(body);
-	const btQuaternion& orn = body->getWorldTransform().getRotation();
-	orientation[0] = orn.getX();
-	orientation[1] = orn.getY();
-	orientation[2] = orn.getZ();
-	orientation[3] = orn.getW();
-}
-
-
-
-//plRigidBodyHandle plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
-
-//	extern  plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
-
-double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3])
-{
-	btVector3 vp(p1[0], p1[1], p1[2]);
-	btTriangleShapeEx trishapeA(vp,
-				  btVector3(p2[0], p2[1], p2[2]), 
-				  btVector3(p3[0], p3[1], p3[2]));
-	trishapeA.setMargin(0.000001f);
-	btVector3 vq(q1[0], q1[1], q1[2]);
-	btTriangleShapeEx trishapeB(vq,
-				  btVector3(q2[0], q2[1], q2[2]), 
-				  btVector3(q3[0], q3[1], q3[2]));
-	trishapeB.setMargin(0.000001f);
-	
-	// btVoronoiSimplexSolver sGjkSimplexSolver;
-	// btGjkEpaPenetrationDepthSolver penSolverPtr;	
-	
-	/*static*/ btSimplexSolverInterface sGjkSimplexSolver;
-	sGjkSimplexSolver.reset();
-	
-	/*static*/ btGjkEpaPenetrationDepthSolver Solver0;
-	/*static*/ btMinkowskiPenetrationDepthSolver Solver1;
-		
-	btConvexPenetrationDepthSolver* Solver = NULL;
-	
-	Solver = &Solver1;	
-		
-	btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,Solver);
-	
-	convexConvex.m_catchDegeneracies = 1;
-	
-	// btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,0);
-	
-	btPointCollector gjkOutput;
-	btGjkPairDetector::ClosestPointInput input;
-	
-		
-	btTransform tr;
-	tr.setIdentity();
-	
-	input.m_transformA = tr;
-	input.m_transformB = tr;
-	
-	convexConvex.getClosestPoints(input, gjkOutput, 0);
-	
-	
-	if (gjkOutput.m_hasResult)
-	{
-		
-		pb[0] = pa[0] = gjkOutput.m_pointInWorld[0];
-		pb[1] = pa[1] = gjkOutput.m_pointInWorld[1];
-		pb[2] = pa[2] = gjkOutput.m_pointInWorld[2];
-
-		pb[0]+= gjkOutput.m_normalOnBInWorld[0] * gjkOutput.m_distance;
-		pb[1]+= gjkOutput.m_normalOnBInWorld[1] * gjkOutput.m_distance;
-		pb[2]+= gjkOutput.m_normalOnBInWorld[2] * gjkOutput.m_distance;
-		
-		normal[0] = gjkOutput.m_normalOnBInWorld[0];
-		normal[1] = gjkOutput.m_normalOnBInWorld[1];
-		normal[2] = gjkOutput.m_normalOnBInWorld[2];
-
-		return gjkOutput.m_distance;
-	}
-	return -1.0f;	
-}
-
-// Convex hull
-plConvexHull plConvexHullCompute(float (*coords)[3], int count)
-{
-	btConvexHullComputer *computer = new btConvexHullComputer;
-	computer->compute(reinterpret_cast< float* >(coords),
-					  sizeof(*coords), count, 0, 0);
-	return reinterpret_cast<plConvexHull>(computer);
-}
-
-void plConvexHullDelete(plConvexHull hull)
-{
-	btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
-	delete computer;
-}
-
-int plConvexHullNumVertices(plConvexHull hull)
-{
-	btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
-	return computer->vertices.size();
-}
-
-int plConvexHullNumFaces(plConvexHull hull)
-{
-	btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
-	return computer->faces.size();
-}
-
-void plConvexHullGetVertex(plConvexHull hull, int n, float coords[3],
-						   int *original_index)
-{
-	btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
-	const btVector3 &v(computer->vertices[n]);
-	coords[0] = v[0];
-	coords[1] = v[1];
-	coords[2] = v[2];
-	(*original_index) = computer->original_vertex_index[n];
-}
-
-int plConvexHullGetFaceSize(plConvexHull hull, int n)
-{
-	btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
-	const btConvexHullComputer::Edge *e_orig, *e;
-	int count;
-
-	for (e_orig = &computer->edges[computer->faces[n]], e = e_orig, count = 0;
-		 count == 0 || e != e_orig;
-		 e = e->getNextEdgeOfFace(), count++);
-	return count;
-}
-
-void plConvexHullGetFaceVertices(plConvexHull hull, int n, int *vertices)
-{
-	btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
-	const btConvexHullComputer::Edge *e_orig, *e;
-	int count;
-
-	for (e_orig = &computer->edges[computer->faces[n]], e = e_orig, count = 0;
-		 count == 0 || e != e_orig;
-		 e = e->getNextEdgeOfFace(), count++)
-	{
-		vertices[count] = e->getTargetVertex();
-	}
-}
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h b/extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h
index e1fea3a49c0..b5cac56cdca 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h
@@ -26,21 +26,16 @@ class btCollisionWorld;
 class btActionInterface
 {
 protected:
-
 	static btRigidBody& getFixedBody();
-	
-	
-public:
 
+public:
 	virtual ~btActionInterface()
 	{
 	}
 
-	virtual void updateAction( btCollisionWorld* collisionWorld, btScalar deltaTimeStep)=0;
+	virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTimeStep) = 0;
 
 	virtual void debugDraw(btIDebugDraw* debugDrawer) = 0;
-
 };
 
-#endif //_BT_ACTION_INTERFACE_H
-
+#endif  //_BT_ACTION_INTERFACE_H
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index 361a054ec69..fb15ae31eb5 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btDiscreteDynamicsWorld.h"
 
 //collision detection
@@ -38,11 +37,9 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
 
-
 #include "LinearMath/btIDebugDraw.h"
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
 
-
 #include "BulletDynamics/Dynamics/btActionInterface.h"
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btMotionState.h"
@@ -56,57 +53,52 @@ int startHit=2;
 int firstHit=startHit;
 #endif
 
-SIMD_FORCE_INLINE	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
+SIMD_FORCE_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();
+	islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
 	return islandId;
-
 }
 
-
 class btSortConstraintOnIslandPredicate
 {
-	public:
-
-		bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const
-		{
-			int rIslandId0,lIslandId0;
-			rIslandId0 = btGetConstraintIslandId(rhs);
-			lIslandId0 = btGetConstraintIslandId(lhs);
-			return lIslandId0 < rIslandId0;
-		}
+public:
+	bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const
+	{
+		int rIslandId0, lIslandId0;
+		rIslandId0 = btGetConstraintIslandId(rhs);
+		lIslandId0 = btGetConstraintIslandId(lhs);
+		return lIslandId0 < rIslandId0;
+	}
 };
 
 struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
 {
-	btContactSolverInfo*	m_solverInfo;
-	btConstraintSolver*		m_solver;
-	btTypedConstraint**		m_sortedConstraints;
-	int						m_numConstraints;
-	btIDebugDraw*			m_debugDrawer;
-	btDispatcher*			m_dispatcher;
+	btContactSolverInfo* m_solverInfo;
+	btConstraintSolver* m_solver;
+	btTypedConstraint** m_sortedConstraints;
+	int m_numConstraints;
+	btIDebugDraw* m_debugDrawer;
+	btDispatcher* m_dispatcher;
 
 	btAlignedObjectArray<btCollisionObject*> m_bodies;
 	btAlignedObjectArray<btPersistentManifold*> m_manifolds;
 	btAlignedObjectArray<btTypedConstraint*> m_constraints;
 
-
 	InplaceSolverIslandCallback(
-		btConstraintSolver*	solver,
+		btConstraintSolver* solver,
 		btStackAlloc* stackAlloc,
 		btDispatcher* dispatcher)
-		:m_solverInfo(NULL),
-		m_solver(solver),
-		m_sortedConstraints(NULL),
-		m_numConstraints(0),
-		m_debugDrawer(NULL),
-		m_dispatcher(dispatcher)
+		: m_solverInfo(NULL),
+		  m_solver(solver),
+		  m_sortedConstraints(NULL),
+		  m_numConstraints(0),
+		  m_debugDrawer(NULL),
+		  m_dispatcher(dispatcher)
 	{
-
 	}
 
 	InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
@@ -116,34 +108,34 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 		return *this;
 	}
 
-	SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints,	int	numConstraints,	btIDebugDraw* debugDrawer)
+	SIMD_FORCE_INLINE void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btIDebugDraw* debugDrawer)
 	{
 		btAssert(solverInfo);
 		m_solverInfo = solverInfo;
 		m_sortedConstraints = sortedConstraints;
 		m_numConstraints = numConstraints;
 		m_debugDrawer = debugDrawer;
-		m_bodies.resize (0);
-		m_manifolds.resize (0);
-		m_constraints.resize (0);
+		m_bodies.resize(0);
+		m_manifolds.resize(0);
+		m_constraints.resize(0);
 	}
 
-
-	virtual	void	processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
+	virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId)
 	{
-		if (islandId<0)
+		if (islandId < 0)
 		{
 			///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
-			m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
-		} else
+			m_solver->solveGroup(bodies, numBodies, manifolds, numManifolds, &m_sortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
+		}
+		else
 		{
-				//also add all non-contact constraints/joints for this island
+			//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++)
+			for (i = 0; i < m_numConstraints; i++)
 			{
 				if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
 				{
@@ -152,7 +144,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 				}
 			}
 			//count the number of constraints in this island
-			for (;i<m_numConstraints;i++)
+			for (; i < m_numConstraints; i++)
 			{
 				if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
 				{
@@ -160,91 +152,87 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 				}
 			}
 
-			if (m_solverInfo->m_minimumSolverBatchSize<=1)
+			if (m_solverInfo->m_minimumSolverBatchSize <= 1)
 			{
-				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
-			} else
+				m_solver->solveGroup(bodies, numBodies, manifolds, numManifolds, startConstraint, numCurConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
+			}
+			else
 			{
-
-				for (i=0;i<numBodies;i++)
+				for (i = 0; i < numBodies; i++)
 					m_bodies.push_back(bodies[i]);
-				for (i=0;i<numManifolds;i++)
+				for (i = 0; i < numManifolds; i++)
 					m_manifolds.push_back(manifolds[i]);
-				for (i=0;i<numCurConstraints;i++)
+				for (i = 0; i < numCurConstraints; i++)
 					m_constraints.push_back(startConstraint[i]);
-				if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
+				if ((m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize)
 				{
 					processConstraints();
-				} else
+				}
+				else
 				{
 					//printf("deferred\n");
 				}
 			}
 		}
 	}
-	void	processConstraints()
+	void processConstraints()
 	{
+		btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
+		btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
+		btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
 
-		btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
-		btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
-		btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
-
-		m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_dispatcher);
+		m_solver->solveGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
 		m_bodies.resize(0);
 		m_manifolds.resize(0);
 		m_constraints.resize(0);
-
 	}
-
 };
 
-
-
-btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
-:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
-m_sortedConstraints	(),
-m_solverIslandCallback ( NULL ),
-m_constraintSolver(constraintSolver),
-m_gravity(0,-10,0),
-m_localTime(0),
-m_fixedTimeStep(0),
-m_synchronizeAllMotionStates(false),
-m_applySpeculativeContactRestitution(false),
-m_profileTimings(0),
-m_latencyMotionStateInterpolation(true)
+btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
+	: btDynamicsWorld(dispatcher, pairCache, collisionConfiguration),
+	  m_sortedConstraints(),
+	  m_solverIslandCallback(NULL),
+	  m_constraintSolver(constraintSolver),
+	  m_gravity(0, -10, 0),
+	  m_localTime(0),
+	  m_fixedTimeStep(0),
+	  m_synchronizeAllMotionStates(false),
+	  m_applySpeculativeContactRestitution(false),
+	  m_profileTimings(0),
+	  m_latencyMotionStateInterpolation(true)
 
 {
 	if (!m_constraintSolver)
 	{
-		void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
+		void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver), 16);
 		m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver;
 		m_ownsConstraintSolver = true;
-	} else
+	}
+	else
 	{
 		m_ownsConstraintSolver = false;
 	}
 
 	{
-		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16);
+		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager), 16);
 		m_islandManager = new (mem) btSimulationIslandManager();
 	}
 
 	m_ownsIslandManager = true;
 
 	{
-		void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback),16);
-		m_solverIslandCallback = new (mem) InplaceSolverIslandCallback (m_constraintSolver, 0, dispatcher);
+		void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback), 16);
+		m_solverIslandCallback = new (mem) InplaceSolverIslandCallback(m_constraintSolver, 0, dispatcher);
 	}
 }
 
-
 btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
 {
 	//only delete it when we created it
 	if (m_ownsIslandManager)
 	{
 		m_islandManager->~btSimulationIslandManager();
-		btAlignedFree( m_islandManager);
+		btAlignedFree(m_islandManager);
 	}
 	if (m_solverIslandCallback)
 	{
@@ -253,18 +241,17 @@ btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
 	}
 	if (m_ownsConstraintSolver)
 	{
-
 		m_constraintSolver->~btConstraintSolver();
 		btAlignedFree(m_constraintSolver);
 	}
 }
 
-void	btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
+void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
 {
-///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
-///to switch status _after_ adding kinematic objects to the world
-///fix it for Bullet 3.x release
-	for (int i=0;i<m_collisionObjects.size();i++)
+	///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
+	///to switch status _after_ adding kinematic objects to the world
+	///fix it for Bullet 3.x release
+	for (int i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -277,10 +264,9 @@ void	btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
 			}
 		}
 	}
-
 }
 
-void	btDiscreteDynamicsWorld::debugDrawWorld()
+void btDiscreteDynamicsWorld::debugDrawWorld()
 {
 	BT_PROFILE("debugDrawWorld");
 
@@ -290,43 +276,40 @@ void	btDiscreteDynamicsWorld::debugDrawWorld()
 	if (getDebugDrawer())
 	{
 		int mode = getDebugDrawer()->getDebugMode();
-		if(mode  & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
+		if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
 		{
 			drawConstraints = true;
 		}
 	}
-	if(drawConstraints)
+	if (drawConstraints)
 	{
-		for(int i = getNumConstraints()-1; i>=0 ;i--)
+		for (int i = getNumConstraints() - 1; i >= 0; i--)
 		{
 			btTypedConstraint* constraint = getConstraint(i);
 			debugDrawConstraint(constraint);
 		}
 	}
 
-
-
-    if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawNormals)))
+	if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawNormals)))
 	{
 		int i;
 
 		if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
 		{
-			for (i=0;i<m_actions.size();i++)
+			for (i = 0; i < m_actions.size(); i++)
 			{
 				m_actions[i]->debugDraw(m_debugDrawer);
 			}
 		}
 	}
-    if (getDebugDrawer())
-        getDebugDrawer()->flushLines();
-
+	if (getDebugDrawer())
+		getDebugDrawer()->flushLines();
 }
 
-void	btDiscreteDynamicsWorld::clearForces()
+void btDiscreteDynamicsWorld::clearForces()
 {
 	///@todo: iterate over awake simulation islands!
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
 	{
 		btRigidBody* body = m_nonStaticRigidBodies[i];
 		//need to check if next line is ok
@@ -336,10 +319,10 @@ void	btDiscreteDynamicsWorld::clearForces()
 }
 
 ///apply gravity, call this once per timestep
-void	btDiscreteDynamicsWorld::applyGravity()
+void btDiscreteDynamicsWorld::applyGravity()
 {
 	///@todo: iterate over awake simulation islands!
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
 	{
 		btRigidBody* body = m_nonStaticRigidBodies[i];
 		if (body->isActive())
@@ -349,8 +332,7 @@ void	btDiscreteDynamicsWorld::applyGravity()
 	}
 }
 
-
-void	btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
+void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
 {
 	btAssert(body);
 
@@ -363,32 +345,32 @@ void	btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
 		{
 			btTransform interpolatedTransform;
 			btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
-				body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),
-				(m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime*body->getHitFraction(),
-				interpolatedTransform);
+												body->getInterpolationLinearVelocity(), body->getInterpolationAngularVelocity(),
+												(m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime * body->getHitFraction(),
+												interpolatedTransform);
 			body->getMotionState()->setWorldTransform(interpolatedTransform);
 		}
 	}
 }
 
-
-void	btDiscreteDynamicsWorld::synchronizeMotionStates()
+void btDiscreteDynamicsWorld::synchronizeMotionStates()
 {
-	BT_PROFILE("synchronizeMotionStates");
+	//	BT_PROFILE("synchronizeMotionStates");
 	if (m_synchronizeAllMotionStates)
 	{
 		//iterate  over all collision objects
-		for ( int i=0;i<m_collisionObjects.size();i++)
+		for (int i = 0; i < m_collisionObjects.size(); i++)
 		{
 			btCollisionObject* colObj = m_collisionObjects[i];
 			btRigidBody* body = btRigidBody::upcast(colObj);
 			if (body)
 				synchronizeSingleMotionState(body);
 		}
-	} else
+	}
+	else
 	{
 		//iterate over all active rigid bodies
-		for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+		for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
 		{
 			btRigidBody* body = m_nonStaticRigidBodies[i];
 			if (body->isActive())
@@ -397,13 +379,10 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 	}
 }
 
-
-int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
+int btDiscreteDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
 {
 	startProfiling(timeStep);
 
-	BT_PROFILE("stepSimulation");
-
 	int numSimulationSubSteps = 0;
 
 	if (maxSubSteps)
@@ -413,10 +392,11 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 		m_localTime += timeStep;
 		if (m_localTime >= fixedTimeStep)
 		{
-			numSimulationSubSteps = int( m_localTime / fixedTimeStep);
+			numSimulationSubSteps = int(m_localTime / fixedTimeStep);
 			m_localTime -= numSimulationSubSteps * fixedTimeStep;
 		}
-	} else
+	}
+	else
 	{
 		//variable timestep
 		fixedTimeStep = timeStep;
@@ -426,7 +406,8 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 		{
 			numSimulationSubSteps = 0;
 			maxSubSteps = 0;
-		} else
+		}
+		else
 		{
 			numSimulationSubSteps = 1;
 			maxSubSteps = 1;
@@ -436,28 +417,25 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 	//process some debugging flags
 	if (getDebugDrawer())
 	{
-		btIDebugDraw* debugDrawer = getDebugDrawer ();
+		btIDebugDraw* debugDrawer = getDebugDrawer();
 		gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
 	}
 	if (numSimulationSubSteps)
 	{
-
 		//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
-		int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
+		int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
 
-		saveKinematicState(fixedTimeStep*clampedSimulationSteps);
+		saveKinematicState(fixedTimeStep * clampedSimulationSteps);
 
 		applyGravity();
 
-
-
-		for (int i=0;i<clampedSimulationSteps;i++)
+		for (int i = 0; i < clampedSimulationSteps; i++)
 		{
 			internalSingleStepSimulation(fixedTimeStep);
 			synchronizeMotionStates();
 		}
-
-	} else
+	}
+	else
 	{
 		synchronizeMotionStates();
 	}
@@ -466,17 +444,17 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 
 #ifndef BT_NO_PROFILE
 	CProfileManager::Increment_Frame_Counter();
-#endif //BT_NO_PROFILE
+#endif  //BT_NO_PROFILE
 
 	return numSimulationSubSteps;
 }
 
-void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
+void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
-
 	BT_PROFILE("internalSingleStepSimulation");
 
-	if(0 != m_internalPreTickCallback) {
+	if (0 != m_internalPreTickCallback)
+	{
 		(*m_internalPreTickCallback)(this, timeStep);
 	}
 
@@ -489,19 +467,15 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 	dispatchInfo.m_stepCount = 0;
 	dispatchInfo.m_debugDraw = getDebugDrawer();
 
-
-    createPredictiveContacts(timeStep);
+	createPredictiveContacts(timeStep);
 
 	///perform collision detection
 	performDiscreteCollisionDetection();
 
 	calculateSimulationIslands();
 
-
 	getSolverInfo().m_timeStep = timeStep;
 
-
-
 	///solve contact and other joint constraints
 	solveConstraints(getSolverInfo());
 
@@ -514,37 +488,38 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 	///update vehicle simulation
 	updateActions(timeStep);
 
-	updateActivationState( timeStep );
+	updateActivationState(timeStep);
 
-	if(0 != m_internalTickCallback) {
+	if (0 != m_internalTickCallback)
+	{
 		(*m_internalTickCallback)(this, timeStep);
 	}
 }
 
-void	btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
+void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
 {
 	m_gravity = gravity;
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
 	{
 		btRigidBody* body = m_nonStaticRigidBodies[i];
-		if (body->isActive() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
+		if (body->isActive() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY))
 		{
 			body->setGravity(gravity);
 		}
 	}
 }
 
-btVector3 btDiscreteDynamicsWorld::getGravity () const
+btVector3 btDiscreteDynamicsWorld::getGravity() const
 {
 	return m_gravity;
 }
 
-void	btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
+void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask)
 {
-	btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask);
+	btCollisionWorld::addCollisionObject(collisionObject, collisionFilterGroup, collisionFilterMask);
 }
 
-void	btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
 {
 	btRigidBody* body = btRigidBody::upcast(collisionObject);
 	if (body)
@@ -553,16 +528,15 @@ void	btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collision
 		btCollisionWorld::removeCollisionObject(collisionObject);
 }
 
-void	btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
+void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
 {
 	m_nonStaticRigidBodies.remove(body);
 	btCollisionWorld::removeCollisionObject(body);
 }
 
-
-void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
+void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
 {
-	if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
+	if (!body->isStaticOrKinematicObject() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY))
 	{
 		body->setGravity(m_gravity);
 	}
@@ -572,22 +546,23 @@ void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
 		if (!body->isStaticObject())
 		{
 			m_nonStaticRigidBodies.push_back(body);
-		} else
+		}
+		else
 		{
 			body->setActivationState(ISLAND_SLEEPING);
 		}
 
 		bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
-		short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
-		short collisionFilterMask = isDynamic? 	short(btBroadphaseProxy::AllFilter) : 	short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+		int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
+		int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
 
-		addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
+		addCollisionObject(body, collisionFilterGroup, collisionFilterMask);
 	}
 }
 
-void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
+void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask)
 {
-	if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))
+	if (!body->isStaticOrKinematicObject() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY))
 	{
 		body->setGravity(m_gravity);
 	}
@@ -598,31 +573,29 @@ void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short
 		{
 			m_nonStaticRigidBodies.push_back(body);
 		}
-		 else
+		else
 		{
 			body->setActivationState(ISLAND_SLEEPING);
 		}
-		addCollisionObject(body,group,mask);
+		addCollisionObject(body, group, mask);
 	}
 }
 
-
-void	btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
+void btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
 {
 	BT_PROFILE("updateActions");
 
-	for ( int i=0;i<m_actions.size();i++)
+	for (int i = 0; i < m_actions.size(); i++)
 	{
-		m_actions[i]->updateAction( this, timeStep);
+		m_actions[i]->updateAction(this, timeStep);
 	}
 }
 
-
-void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
+void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 {
 	BT_PROFILE("updateActivationState");
 
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
 	{
 		btRigidBody* body = m_nonStaticRigidBodies[i];
 		if (body)
@@ -634,32 +607,33 @@ void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 				if (body->isStaticOrKinematicObject())
 				{
 					body->setActivationState(ISLAND_SLEEPING);
-				} else
+				}
+				else
 				{
 					if (body->getActivationState() == ACTIVE_TAG)
-						body->setActivationState( WANTS_DEACTIVATION );
+						body->setActivationState(WANTS_DEACTIVATION);
 					if (body->getActivationState() == ISLAND_SLEEPING)
 					{
-						body->setAngularVelocity(btVector3(0,0,0));
-						body->setLinearVelocity(btVector3(0,0,0));
+						body->setAngularVelocity(btVector3(0, 0, 0));
+						body->setLinearVelocity(btVector3(0, 0, 0));
 					}
-
 				}
-			} else
+			}
+			else
 			{
 				if (body->getActivationState() != DISABLE_DEACTIVATION)
-					body->setActivationState( ACTIVE_TAG );
+					body->setActivationState(ACTIVE_TAG);
 			}
 		}
 	}
 }
 
-void	btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
+void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies)
 {
 	m_constraints.push_back(constraint);
-    //Make sure the two bodies of a type constraint are different (possibly add this to the btTypedConstraint constructor?)
-    btAssert(&constraint->getRigidBodyA()!=&constraint->getRigidBodyB());
-    
+	//Make sure the two bodies of a type constraint are different (possibly add this to the btTypedConstraint constructor?)
+	btAssert(&constraint->getRigidBodyA() != &constraint->getRigidBodyB());
+
 	if (disableCollisionsBetweenLinkedBodies)
 	{
 		constraint->getRigidBodyA().addConstraintRef(constraint);
@@ -667,105 +641,98 @@ void	btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool d
 	}
 }
 
-void	btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
+void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
 {
 	m_constraints.remove(constraint);
 	constraint->getRigidBodyA().removeConstraintRef(constraint);
 	constraint->getRigidBodyB().removeConstraintRef(constraint);
 }
 
-void	btDiscreteDynamicsWorld::addAction(btActionInterface* action)
+void btDiscreteDynamicsWorld::addAction(btActionInterface* action)
 {
 	m_actions.push_back(action);
 }
 
-void	btDiscreteDynamicsWorld::removeAction(btActionInterface* action)
+void btDiscreteDynamicsWorld::removeAction(btActionInterface* action)
 {
 	m_actions.remove(action);
 }
 
-
-void	btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)
+void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)
 {
 	addAction(vehicle);
 }
 
-void	btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)
+void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)
 {
 	removeAction(vehicle);
 }
 
-void	btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)
+void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)
 {
 	addAction(character);
 }
 
-void	btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
+void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
 {
 	removeAction(character);
 }
 
-
-
-
-void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
+void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
 	BT_PROFILE("solveConstraints");
 
-	m_sortedConstraints.resize( m_constraints.size());
+	m_sortedConstraints.resize(m_constraints.size());
 	int i;
-	for (i=0;i<getNumConstraints();i++)
+	for (i = 0; i < getNumConstraints(); i++)
 	{
 		m_sortedConstraints[i] = m_constraints[i];
 	}
 
-//	btAssert(0);
-
-
+	//	btAssert(0);
 
 	m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
 
 	btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
 
-	m_solverIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),getDebugDrawer());
+	m_solverIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), getDebugDrawer());
 	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
 
 	/// solve all the constraints for this island
-	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverIslandCallback);
+	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverIslandCallback);
 
 	m_solverIslandCallback->processConstraints();
 
 	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
 }
 
-
-void	btDiscreteDynamicsWorld::calculateSimulationIslands()
+void btDiscreteDynamicsWorld::calculateSimulationIslands()
 {
 	BT_PROFILE("calculateSimulationIslands");
 
-	getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
+	getSimulationIslandManager()->updateActivationState(getCollisionWorld(), getCollisionWorld()->getDispatcher());
 
-    {
-        //merge islands based on speculative contact manifolds too
-        for (int i=0;i<this->m_predictiveManifolds.size();i++)
-        {
-            btPersistentManifold* manifold = m_predictiveManifolds[i];
+	{
+		//merge islands based on speculative contact manifolds too
+		for (int i = 0; i < this->m_predictiveManifolds.size(); i++)
+		{
+			btPersistentManifold* manifold = m_predictiveManifolds[i];
 
-            const btCollisionObject* colObj0 = manifold->getBody0();
-            const btCollisionObject* colObj1 = manifold->getBody1();
+			const btCollisionObject* colObj0 = manifold->getBody0();
+			const btCollisionObject* colObj1 = manifold->getBody1();
 
-            if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
-                ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
-            {
-				getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
-            }
-        }
-    }
+			if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
+				((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
+			{
+				getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
+			}
+		}
+	}
 
 	{
 		int i;
 		int numConstraints = int(m_constraints.size());
-		for (i=0;i< numConstraints ; i++ )
+		for (i = 0; i < numConstraints; i++)
 		{
 			btTypedConstraint* constraint = m_constraints[i];
 			if (constraint->isEnabled())
@@ -776,7 +743,7 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
 				if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
 					((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
 				{
-					getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
+					getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
 				}
 			}
 		}
@@ -784,51 +751,44 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
 
 	//Store the island id in each body
 	getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
-
-
 }
 
-
-
-
 class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
 {
 public:
-
 	btCollisionObject* m_me;
 	btScalar m_allowedPenetration;
 	btOverlappingPairCache* m_pairCache;
 	btDispatcher* m_dispatcher;
 
 public:
-	btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
-	  btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
-		m_me(me),
-		m_allowedPenetration(0.0f),
-		m_pairCache(pairCache),
-		m_dispatcher(dispatcher)
+	btClosestNotMeConvexResultCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btCollisionWorld::ClosestConvexResultCallback(fromA, toA),
+																																										   m_me(me),
+																																										   m_allowedPenetration(0.0f),
+																																										   m_pairCache(pairCache),
+																																										   m_dispatcher(dispatcher)
 	{
 	}
 
-	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
+	virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace)
 	{
 		if (convexResult.m_hitCollisionObject == m_me)
 			return 1.0f;
 
 		//ignore result if there is no contact response
-		if(!convexResult.m_hitCollisionObject->hasContactResponse())
+		if (!convexResult.m_hitCollisionObject->hasContactResponse())
 			return 1.0f;
 
-		btVector3 linVelA,linVelB;
-		linVelA = m_convexToWorld-m_convexFromWorld;
-		linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin();
+		btVector3 linVelA, linVelB;
+		linVelA = m_convexToWorld - m_convexFromWorld;
+		linVelB = btVector3(0, 0, 0);  //toB.getOrigin()-fromB.getOrigin();
 
-		btVector3 relativeVelocity = (linVelA-linVelB);
+		btVector3 relativeVelocity = (linVelA - linVelB);
 		//don't report time of impact for motion away from the contact normal (or causes minor penetration)
-		if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration)
+		if (convexResult.m_hitNormalLocal.dot(relativeVelocity) >= -m_allowedPenetration)
 			return 1.f;
 
-		return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
+		return ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace);
 	}
 
 	virtual bool needsCollision(btBroadphaseProxy* proxy0) const
@@ -840,11 +800,22 @@ public:
 		///don't do CCD when the collision filters are not matching
 		if (!ClosestConvexResultCallback::needsCollision(proxy0))
 			return false;
+		if (m_pairCache->getOverlapFilterCallback()) {
+			btBroadphaseProxy* proxy1 = m_me->getBroadphaseHandle();
+			bool collides = m_pairCache->needsBroadphaseCollision(proxy0, proxy1);
+			if (!collides)
+			{
+				return false;
+			}
+		}
 
-		btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
+		btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject;
+
+		if (!m_dispatcher->needsCollision(m_me, otherObj))
+			return false;
 
 		//call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
-		if (m_dispatcher->needsResponse(m_me,otherObj))
+		if (m_dispatcher->needsResponse(m_me, otherObj))
 		{
 #if 0
 			///don't do CCD when there are already contact points (touching contact/penetration)
@@ -870,41 +841,24 @@ public:
 
 		return false;
 	}
-
-
 };
 
 ///internal debugging variable. this value shouldn't be too high
-int gNumClampedCcdMotions=0;
+int gNumClampedCcdMotions = 0;
 
-
-void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
+void btDiscreteDynamicsWorld::createPredictiveContactsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep)
 {
-	BT_PROFILE("createPredictiveContacts");
-
-	{
-		BT_PROFILE("release predictive contact manifolds");
-
-		for (int i=0;i<m_predictiveManifolds.size();i++)
-		{
-			btPersistentManifold* manifold = m_predictiveManifolds[i];
-			this->m_dispatcher1->releaseManifold(manifold);
-		}
-		m_predictiveManifolds.clear();
-	}
-
 	btTransform predictedTrans;
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	for (int i = 0; i < numBodies; i++)
 	{
-		btRigidBody* body = m_nonStaticRigidBodies[i];
+		btRigidBody* body = bodies[i];
 		body->setHitFraction(1.f);
 
 		if (body->isActive() && (!body->isStaticOrKinematicObject()))
 		{
-
 			body->predictIntegratedTransform(timeStep, predictedTrans);
 
-			btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
+			btScalar squareMotion = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()).length2();
 
 			if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
 			{
@@ -916,81 +870,97 @@ void	btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
 					class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
 					{
 					public:
-
-						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
-						  btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
+						StaticOnlyCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me, fromA, toA, pairCache, dispatcher)
 						{
 						}
 
-					  	virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+						virtual bool needsCollision(btBroadphaseProxy* proxy0) const
 						{
-							btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
+							btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject;
 							if (!otherObj->isStaticOrKinematicObject())
 								return false;
 							return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
 						}
 					};
 
-					StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
+					StaticOnlyCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher());
 #else
-					btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
+					btClosestNotMeConvexResultCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher());
 #endif
 					//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
-					btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
-					sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration;
+					btSphereShape tmpSphere(body->getCcdSweptSphereRadius());  //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+					sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration;
 
 					sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
-					sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
+					sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
 					btTransform modifiedPredictedTrans = predictedTrans;
 					modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
 
-					convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
+					convexSweepTest(&tmpSphere, body->getWorldTransform(), modifiedPredictedTrans, sweepResults);
 					if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
 					{
-
-						btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction;
+						btVector3 distVec = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()) * sweepResults.m_closestHitFraction;
 						btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld);
 
-
-						btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject);
+						btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body, sweepResults.m_hitCollisionObject);
+						btMutexLock(&m_predictiveManifoldsMutex);
 						m_predictiveManifolds.push_back(manifold);
+						btMutexUnlock(&m_predictiveManifoldsMutex);
 
-						btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec;
-						btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB;
+						btVector3 worldPointB = body->getWorldTransform().getOrigin() + distVec;
+						btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse() * worldPointB;
 
-						btManifoldPoint newPoint(btVector3(0,0,0), localPointB,sweepResults.m_hitNormalWorld,distance);
+						btManifoldPoint newPoint(btVector3(0, 0, 0), localPointB, sweepResults.m_hitNormalWorld, distance);
 
 						bool isPredictive = true;
 						int index = manifold->addManifoldPoint(newPoint, isPredictive);
 						btManifoldPoint& pt = manifold->getContactPoint(index);
 						pt.m_combinedRestitution = 0;
-						pt.m_combinedFriction = btManifoldResult::calculateCombinedFriction(body,sweepResults.m_hitCollisionObject);
+						pt.m_combinedFriction = gCalculateCombinedFrictionCallback(body, sweepResults.m_hitCollisionObject);
 						pt.m_positionWorldOnA = body->getWorldTransform().getOrigin();
 						pt.m_positionWorldOnB = worldPointB;
-
 					}
 				}
 			}
 		}
 	}
 }
-void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
+
+void btDiscreteDynamicsWorld::releasePredictiveContacts()
+{
+	BT_PROFILE("release predictive contact manifolds");
+
+	for (int i = 0; i < m_predictiveManifolds.size(); i++)
+	{
+		btPersistentManifold* manifold = m_predictiveManifolds[i];
+		this->m_dispatcher1->releaseManifold(manifold);
+	}
+	m_predictiveManifolds.clear();
+}
+
+void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
+{
+	BT_PROFILE("createPredictiveContacts");
+	releasePredictiveContacts();
+	if (m_nonStaticRigidBodies.size() > 0)
+	{
+		createPredictiveContactsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep);
+	}
+}
+
+void btDiscreteDynamicsWorld::integrateTransformsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep)
 {
-	BT_PROFILE("integrateTransforms");
 	btTransform predictedTrans;
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	for (int i = 0; i < numBodies; i++)
 	{
-		btRigidBody* body = m_nonStaticRigidBodies[i];
+		btRigidBody* body = bodies[i];
 		body->setHitFraction(1.f);
 
 		if (body->isActive() && (!body->isStaticOrKinematicObject()))
 		{
-
 			body->predictIntegratedTransform(timeStep, predictedTrans);
 
-			btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
-
-
+			btScalar squareMotion = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()).length2();
 
 			if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
 			{
@@ -1002,43 +972,40 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 					class StaticOnlyCallback : public btClosestNotMeConvexResultCallback
 					{
 					public:
-
-						StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
-						  btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)
+						StaticOnlyCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me, fromA, toA, pairCache, dispatcher)
 						{
 						}
 
-					  	virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+						virtual bool needsCollision(btBroadphaseProxy* proxy0) const
 						{
-							btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
+							btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject;
 							if (!otherObj->isStaticOrKinematicObject())
 								return false;
 							return btClosestNotMeConvexResultCallback::needsCollision(proxy0);
 						}
 					};
 
-					StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
+					StaticOnlyCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher());
 #else
-					btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
+					btClosestNotMeConvexResultCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher());
 #endif
 					//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
-					btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
-					sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration;
+					btSphereShape tmpSphere(body->getCcdSweptSphereRadius());  //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
+					sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration;
 
 					sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
-					sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
+					sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
 					btTransform modifiedPredictedTrans = predictedTrans;
 					modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
 
-					convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
+					convexSweepTest(&tmpSphere, body->getWorldTransform(), modifiedPredictedTrans, sweepResults);
 					if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
 					{
-
 						//printf("clamped integration to hit fraction = %f\n",fraction);
 						body->setHitFraction(sweepResults.m_closestHitFraction);
-						body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
+						body->predictIntegratedTransform(timeStep * body->getHitFraction(), predictedTrans);
 						body->setHitFraction(0.f);
-						body->proceedToTransform( predictedTrans);
+						body->proceedToTransform(predictedTrans);
 
 #if 0
 						btVector3 linVel = body->getLinearVelocity();
@@ -1065,40 +1032,45 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 						//btScalar depth = 0.f;
 						//appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
 
-
 #endif
 
-        				continue;
+						continue;
 					}
 				}
 			}
 
-
-			body->proceedToTransform( predictedTrans);
-
+			body->proceedToTransform(predictedTrans);
 		}
+	}
+}
 
+void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
+{
+	BT_PROFILE("integrateTransforms");
+	if (m_nonStaticRigidBodies.size() > 0)
+	{
+		integrateTransformsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep);
 	}
 
 	///this should probably be switched on by default, but it is not well tested yet
 	if (m_applySpeculativeContactRestitution)
 	{
 		BT_PROFILE("apply speculative contact restitution");
-		for (int i=0;i<m_predictiveManifolds.size();i++)
+		for (int i = 0; i < m_predictiveManifolds.size(); i++)
 		{
 			btPersistentManifold* manifold = m_predictiveManifolds[i];
 			btRigidBody* body0 = btRigidBody::upcast((btCollisionObject*)manifold->getBody0());
 			btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1());
 
-			for (int p=0;p<manifold->getNumContacts();p++)
+			for (int p = 0; p < manifold->getNumContacts(); p++)
 			{
 				const btManifoldPoint& pt = manifold->getContactPoint(p);
-				btScalar combinedRestitution = btManifoldResult::calculateCombinedRestitution(body0, body1);
+				btScalar combinedRestitution = gCalculateCombinedRestitutionCallback(body0, body1);
 
-				if (combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
+				if (combinedRestitution > 0 && pt.m_appliedImpulse != 0.f)
 				//if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
 				{
-					btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse* combinedRestitution;
+					btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse * combinedRestitution;
 
 					const btVector3& pos1 = pt.getPositionWorldOnA();
 					const btVector3& pos2 = pt.getPositionWorldOnB();
@@ -1107,25 +1079,19 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 					btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin();
 
 					if (body0)
-						body0->applyImpulse(imp,rel_pos0);
+						body0->applyImpulse(imp, rel_pos0);
 					if (body1)
-						body1->applyImpulse(-imp,rel_pos1);
+						body1->applyImpulse(-imp, rel_pos1);
 				}
 			}
 		}
 	}
-
 }
 
-
-
-
-
-
-void	btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 {
 	BT_PROFILE("predictUnconstraintMotion");
-	for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
+	for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
 	{
 		btRigidBody* body = m_nonStaticRigidBodies[i];
 		if (!body->isStaticOrKinematicObject())
@@ -1134,179 +1100,171 @@ void	btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 
 			body->applyDamping(timeStep);
 
-			body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
+			body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform());
 		}
 	}
 }
 
-
-void	btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
+void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
 {
 	(void)timeStep;
 
 #ifndef BT_NO_PROFILE
 	CProfileManager::Reset();
-#endif //BT_NO_PROFILE
-
+#endif  //BT_NO_PROFILE
 }
 
-
-
-
-
-
 void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 {
 	bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
 	bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
 	btScalar dbgDrawSize = constraint->getDbgDrawSize();
-	if(dbgDrawSize <= btScalar(0.f))
+	if (dbgDrawSize <= btScalar(0.f))
 	{
 		return;
 	}
 
-	switch(constraint->getConstraintType())
+	switch (constraint->getConstraintType())
 	{
 		case POINT2POINT_CONSTRAINT_TYPE:
+		{
+			btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
+			btTransform tr;
+			tr.setIdentity();
+			btVector3 pivot = p2pC->getPivotInA();
+			pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
+			tr.setOrigin(pivot);
+			getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			// that ideally should draw the same frame
+			pivot = p2pC->getPivotInB();
+			pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
+			tr.setOrigin(pivot);
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+		}
+		break;
+		case HINGE_CONSTRAINT_TYPE:
+		{
+			btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
+			btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			btScalar minAng = pHinge->getLowerLimit();
+			btScalar maxAng = pHinge->getUpperLimit();
+			if (minAng == maxAng)
 			{
-				btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
-				btTransform tr;
-				tr.setIdentity();
-				btVector3 pivot = p2pC->getPivotInA();
-				pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
-				tr.setOrigin(pivot);
-				getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				// that ideally should draw the same frame
-				pivot = p2pC->getPivotInB();
-				pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
-				tr.setOrigin(pivot);
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				break;
 			}
-			break;
-		case HINGE_CONSTRAINT_TYPE:
+			bool drawSect = true;
+			if (!pHinge->hasLimit())
 			{
-				btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
-				btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				btScalar minAng = pHinge->getLowerLimit();
-				btScalar maxAng = pHinge->getUpperLimit();
-				if(minAng == maxAng)
-				{
-					break;
-				}
-				bool drawSect = true;
-				if(!pHinge->hasLimit())
-				{
-					minAng = btScalar(0.f);
-					maxAng = SIMD_2_PI;
-					drawSect = false;
-				}
-				if(drawLimits)
-				{
-					btVector3& center = tr.getOrigin();
-					btVector3 normal = tr.getBasis().getColumn(2);
-					btVector3 axis = tr.getBasis().getColumn(0);
-					getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect);
-				}
+				minAng = btScalar(0.f);
+				maxAng = SIMD_2_PI;
+				drawSect = false;
 			}
-			break;
+			if (drawLimits)
+			{
+				btVector3& center = tr.getOrigin();
+				btVector3 normal = tr.getBasis().getColumn(2);
+				btVector3 axis = tr.getBasis().getColumn(0);
+				getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0, 0, 0), drawSect);
+			}
+		}
+		break;
 		case CONETWIST_CONSTRAINT_TYPE:
+		{
+			btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
+			btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			if (drawLimits)
 			{
-				btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
-				btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				if(drawLimits)
+				//const btScalar length = btScalar(5);
+				const btScalar length = dbgDrawSize;
+				static int nSegments = 8 * 4;
+				btScalar fAngleInRadians = btScalar(2. * 3.1415926) * (btScalar)(nSegments - 1) / btScalar(nSegments);
+				btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
+				pPrev = tr * pPrev;
+				for (int i = 0; i < nSegments; i++)
 				{
-					//const btScalar length = btScalar(5);
-					const btScalar length = dbgDrawSize;
-					static int nSegments = 8*4;
-					btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments);
-					btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
-					pPrev = tr * pPrev;
-					for (int i=0; i<nSegments; i++)
-					{
-						fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments);
-						btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
-						pCur = tr * pCur;
-						getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0));
+					fAngleInRadians = btScalar(2. * 3.1415926) * (btScalar)i / btScalar(nSegments);
+					btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
+					pCur = tr * pCur;
+					getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0, 0, 0));
 
-						if (i%(nSegments/8) == 0)
-							getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
-
-						pPrev = pCur;
-					}
-					btScalar tws = pCT->getTwistSpan();
-					btScalar twa = pCT->getTwistAngle();
-					bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
-					if(useFrameB)
-					{
-						tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
-					}
-					else
-					{
-						tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
-					}
-					btVector3 pivot = tr.getOrigin();
-					btVector3 normal = tr.getBasis().getColumn(0);
-					btVector3 axis1 = tr.getBasis().getColumn(1);
-					getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true);
+					if (i % (nSegments / 8) == 0)
+						getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0, 0, 0));
 
+					pPrev = pCur;
 				}
+				btScalar tws = pCT->getTwistSpan();
+				btScalar twa = pCT->getTwistAngle();
+				bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
+				if (useFrameB)
+				{
+					tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
+				}
+				else
+				{
+					tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
+				}
+				btVector3 pivot = tr.getOrigin();
+				btVector3 normal = tr.getBasis().getColumn(0);
+				btVector3 axis1 = tr.getBasis().getColumn(1);
+				getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa - tws, -twa + tws, btVector3(0, 0, 0), true);
 			}
-			break;
+		}
+		break;
 		case D6_SPRING_CONSTRAINT_TYPE:
 		case D6_CONSTRAINT_TYPE:
+		{
+			btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
+			btTransform tr = p6DOF->getCalculatedTransformA();
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			tr = p6DOF->getCalculatedTransformB();
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			if (drawLimits)
 			{
-				btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
-				btTransform tr = p6DOF->getCalculatedTransformA();
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				tr = p6DOF->getCalculatedTransformA();
+				const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
+				btVector3 up = tr.getBasis().getColumn(2);
+				btVector3 axis = tr.getBasis().getColumn(0);
+				btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
+				btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
+				btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
+				btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
+				getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
+				axis = tr.getBasis().getColumn(1);
+				btScalar ay = p6DOF->getAngle(1);
+				btScalar az = p6DOF->getAngle(2);
+				btScalar cy = btCos(ay);
+				btScalar sy = btSin(ay);
+				btScalar cz = btCos(az);
+				btScalar sz = btSin(az);
+				btVector3 ref;
+				ref[0] = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2];
+				ref[1] = -sz * axis[0] + cz * axis[1];
+				ref[2] = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2];
 				tr = p6DOF->getCalculatedTransformB();
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				if(drawLimits)
+				btVector3 normal = -tr.getBasis().getColumn(0);
+				btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
+				btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
+				if (minFi > maxFi)
 				{
-					tr = p6DOF->getCalculatedTransformA();
-					const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
-					btVector3 up = tr.getBasis().getColumn(2);
-					btVector3 axis = tr.getBasis().getColumn(0);
-					btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
-					btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
-					btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
-					btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
-					getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
-					axis = tr.getBasis().getColumn(1);
-					btScalar ay = p6DOF->getAngle(1);
-					btScalar az = p6DOF->getAngle(2);
-					btScalar cy = btCos(ay);
-					btScalar sy = btSin(ay);
-					btScalar cz = btCos(az);
-					btScalar sz = btSin(az);
-					btVector3 ref;
-					ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
-					ref[1] = -sz*axis[0] + cz*axis[1];
-					ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
-					tr = p6DOF->getCalculatedTransformB();
-					btVector3 normal = -tr.getBasis().getColumn(0);
-					btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
-					btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
-					if(minFi > maxFi)
-					{
-						getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false);
-					}
-					else if(minFi < maxFi)
-					{
-						getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true);
-					}
-					tr = p6DOF->getCalculatedTransformA();
-					btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
-					btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
-					getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0));
+					getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false);
+				}
+				else if (minFi < maxFi)
+				{
+					getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true);
 				}
+				tr = p6DOF->getCalculatedTransformA();
+				btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
+				btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
+				getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0));
 			}
-			break;
+		}
+		break;
 		///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage
 		case D6_SPRING_2_CONSTRAINT_TYPE:
 		{
@@ -1324,9 +1282,12 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 					btVector3 axis = tr.getBasis().getColumn(0);
 					btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
 					btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
-					btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
-					btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
-					getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
+					if (minTh <= maxTh)
+					{
+						btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
+						btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
+						getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
+					}
 					axis = tr.getBasis().getColumn(1);
 					btScalar ay = p6DOF->getAngle(1);
 					btScalar az = p6DOF->getAngle(2);
@@ -1335,9 +1296,9 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 					btScalar cz = btCos(az);
 					btScalar sz = btSin(az);
 					btVector3 ref;
-					ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
-					ref[1] = -sz*axis[0] + cz*axis[1];
-					ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
+					ref[0] = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2];
+					ref[1] = -sz * axis[0] + cz * axis[1];
+					ref[2] = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2];
 					tr = p6DOF->getCalculatedTransformB();
 					btVector3 normal = -tr.getBasis().getColumn(0);
 					btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
@@ -1359,42 +1320,38 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 			break;
 		}
 		case SLIDER_CONSTRAINT_TYPE:
+		{
+			btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
+			btTransform tr = pSlider->getCalculatedTransformA();
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			tr = pSlider->getCalculatedTransformB();
+			if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+			if (drawLimits)
 			{
-				btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
-				btTransform tr = pSlider->getCalculatedTransformA();
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				tr = pSlider->getCalculatedTransformB();
-				if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
-				if(drawLimits)
-				{
-					btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB();
-					btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
-					btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
-					getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
-					btVector3 normal = tr.getBasis().getColumn(0);
-					btVector3 axis = tr.getBasis().getColumn(1);
-					btScalar a_min = pSlider->getLowerAngLimit();
-					btScalar a_max = pSlider->getUpperAngLimit();
-					const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
-					getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true);
-				}
+				btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB();
+				btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
+				btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
+				getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
+				btVector3 normal = tr.getBasis().getColumn(0);
+				btVector3 axis = tr.getBasis().getColumn(1);
+				btScalar a_min = pSlider->getLowerAngLimit();
+				btScalar a_max = pSlider->getUpperAngLimit();
+				const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
+				getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0, 0, 0), true);
 			}
-			break;
-		default :
+		}
+		break;
+		default:
 			break;
 	}
 	return;
 }
 
-
-
-
-
-void	btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
+void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 {
 	if (m_ownsConstraintSolver)
 	{
-		btAlignedFree( m_constraintSolver);
+		btAlignedFree(m_constraintSolver);
 	}
 	m_ownsConstraintSolver = false;
 	m_constraintSolver = solver;
@@ -1406,8 +1363,7 @@ btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
 	return m_constraintSolver;
 }
 
-
-int		btDiscreteDynamicsWorld::getNumConstraints() const
+int btDiscreteDynamicsWorld::getNumConstraints() const
 {
 	return int(m_constraints.size());
 }
@@ -1420,90 +1376,85 @@ const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
 	return m_constraints[index];
 }
 
-
-
-void	btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer)
+void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer)
 {
 	int i;
 	//serialize all collision objects
-	for (i=0;i<m_collisionObjects.size();i++)
+	for (i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
 		{
 			int len = colObj->calculateSerializeBufferSize();
-			btChunk* chunk = serializer->allocate(len,1);
+			btChunk* chunk = serializer->allocate(len, 1);
 			const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
-			serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,colObj);
+			serializer->finalizeChunk(chunk, structType, BT_RIGIDBODY_CODE, colObj);
 		}
 	}
 
-	for (i=0;i<m_constraints.size();i++)
+	for (i = 0; i < m_constraints.size(); i++)
 	{
 		btTypedConstraint* constraint = m_constraints[i];
 		int size = constraint->calculateSerializeBufferSize();
-		btChunk* chunk = serializer->allocate(size,1);
-		const char* structType = constraint->serialize(chunk->m_oldPtr,serializer);
-		serializer->finalizeChunk(chunk,structType,BT_CONSTRAINT_CODE,constraint);
+		btChunk* chunk = serializer->allocate(size, 1);
+		const char* structType = constraint->serialize(chunk->m_oldPtr, serializer);
+		serializer->finalizeChunk(chunk, structType, BT_CONSTRAINT_CODE, constraint);
 	}
 }
 
-
-
-
-void	btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serializer)
+void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serializer)
 {
 #ifdef BT_USE_DOUBLE_PRECISION
-		int len = sizeof(btDynamicsWorldDoubleData);
-		btChunk* chunk = serializer->allocate(len,1);
-		btDynamicsWorldDoubleData* worldInfo = (btDynamicsWorldDoubleData*)chunk->m_oldPtr;
-#else//BT_USE_DOUBLE_PRECISION
-		int len = sizeof(btDynamicsWorldFloatData);
-		btChunk* chunk = serializer->allocate(len,1);
-		btDynamicsWorldFloatData* worldInfo = (btDynamicsWorldFloatData*)chunk->m_oldPtr;
-#endif//BT_USE_DOUBLE_PRECISION
-
-		memset(worldInfo ,0x00,len);
-
-		m_gravity.serialize(worldInfo->m_gravity);
-		worldInfo->m_solverInfo.m_tau = getSolverInfo().m_tau;
-		worldInfo->m_solverInfo.m_damping = getSolverInfo().m_damping;
-		worldInfo->m_solverInfo.m_friction = getSolverInfo().m_friction;
-		worldInfo->m_solverInfo.m_timeStep = getSolverInfo().m_timeStep;
-
-		worldInfo->m_solverInfo.m_restitution = getSolverInfo().m_restitution;
-		worldInfo->m_solverInfo.m_maxErrorReduction = getSolverInfo().m_maxErrorReduction;
-		worldInfo->m_solverInfo.m_sor = getSolverInfo().m_sor;
-		worldInfo->m_solverInfo.m_erp = getSolverInfo().m_erp;
-
-		worldInfo->m_solverInfo.m_erp2 = getSolverInfo().m_erp2;
-		worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm;
-		worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold;
-		worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp;
-
-		worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop;
-		worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor;
-		worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce;
-		worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold;
-
-		worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations;
-		worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode;
-		worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold;
-		worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize;
-
-		worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse;
-
+	int len = sizeof(btDynamicsWorldDoubleData);
+	btChunk* chunk = serializer->allocate(len, 1);
+	btDynamicsWorldDoubleData* worldInfo = (btDynamicsWorldDoubleData*)chunk->m_oldPtr;
+#else   //BT_USE_DOUBLE_PRECISION
+	int len = sizeof(btDynamicsWorldFloatData);
+	btChunk* chunk = serializer->allocate(len, 1);
+	btDynamicsWorldFloatData* worldInfo = (btDynamicsWorldFloatData*)chunk->m_oldPtr;
+#endif  //BT_USE_DOUBLE_PRECISION
+
+	memset(worldInfo, 0x00, len);
+
+	m_gravity.serialize(worldInfo->m_gravity);
+	worldInfo->m_solverInfo.m_tau = getSolverInfo().m_tau;
+	worldInfo->m_solverInfo.m_damping = getSolverInfo().m_damping;
+	worldInfo->m_solverInfo.m_friction = getSolverInfo().m_friction;
+	worldInfo->m_solverInfo.m_timeStep = getSolverInfo().m_timeStep;
+
+	worldInfo->m_solverInfo.m_restitution = getSolverInfo().m_restitution;
+	worldInfo->m_solverInfo.m_maxErrorReduction = getSolverInfo().m_maxErrorReduction;
+	worldInfo->m_solverInfo.m_sor = getSolverInfo().m_sor;
+	worldInfo->m_solverInfo.m_erp = getSolverInfo().m_erp;
+
+	worldInfo->m_solverInfo.m_erp2 = getSolverInfo().m_erp2;
+	worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm;
+	worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold;
+	worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp;
+
+	worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop;
+	worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor;
+	worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce;
+	worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold;
+
+	worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations;
+	worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode;
+	worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold;
+	worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize;
+
+	worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse;
+
+	
 #ifdef BT_USE_DOUBLE_PRECISION
-		const char* structType = "btDynamicsWorldDoubleData";
-#else//BT_USE_DOUBLE_PRECISION
-		const char* structType = "btDynamicsWorldFloatData";
-#endif//BT_USE_DOUBLE_PRECISION
-		serializer->finalizeChunk(chunk,structType,BT_DYNAMICSWORLD_CODE,worldInfo);
+	const char* structType = "btDynamicsWorldDoubleData";
+#else   //BT_USE_DOUBLE_PRECISION
+	const char* structType = "btDynamicsWorldFloatData";
+#endif  //BT_USE_DOUBLE_PRECISION
+	serializer->finalizeChunk(chunk, structType, BT_DYNAMICSWORLD_CODE, worldInfo);
 }
 
-void	btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
+void btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
 {
-
 	serializer->startSerialization();
 
 	serializeDynamicsWorldInfo(serializer);
@@ -1512,6 +1463,7 @@ void	btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
 
 	serializeRigidBodies(serializer);
 
+	serializeContactManifolds(serializer);
+
 	serializer->finishSerialization();
 }
-
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
index dd3d1c3660c..73607c61fd5 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
@@ -13,12 +13,10 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #ifndef BT_DISCRETE_DYNAMICS_WORLD_H
 #define BT_DISCRETE_DYNAMICS_WORLD_H
 
 #include "btDynamicsWorld.h"
-
 class btDispatcher;
 class btOverlappingPairCache;
 class btConstraintSolver;
@@ -27,159 +25,161 @@ class btTypedConstraint;
 class btActionInterface;
 class btPersistentManifold;
 class btIDebugDraw;
+
 struct InplaceSolverIslandCallback;
 
 #include "LinearMath/btAlignedObjectArray.h"
-
+#include "LinearMath/btThreads.h"
 
 ///btDiscreteDynamicsWorld provides discrete rigid body simulation
 ///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
-ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorld : public btDynamicsWorld
+ATTRIBUTE_ALIGNED16(class)
+btDiscreteDynamicsWorld : public btDynamicsWorld
 {
 protected:
-	
-    btAlignedObjectArray<btTypedConstraint*>	m_sortedConstraints;
-	InplaceSolverIslandCallback* 	m_solverIslandCallback;
+	btAlignedObjectArray<btTypedConstraint*> m_sortedConstraints;
+	InplaceSolverIslandCallback* m_solverIslandCallback;
 
-	btConstraintSolver*	m_constraintSolver;
+	btConstraintSolver* m_constraintSolver;
 
-	btSimulationIslandManager*	m_islandManager;
+	btSimulationIslandManager* m_islandManager;
 
 	btAlignedObjectArray<btTypedConstraint*> m_constraints;
 
 	btAlignedObjectArray<btRigidBody*> m_nonStaticRigidBodies;
 
-	btVector3	m_gravity;
+	btVector3 m_gravity;
 
 	//for variable timesteps
-	btScalar	m_localTime;
-	btScalar	m_fixedTimeStep;
+	btScalar m_localTime;
+	btScalar m_fixedTimeStep;
 	//for variable timesteps
 
-	bool	m_ownsIslandManager;
-	bool	m_ownsConstraintSolver;
-	bool	m_synchronizeAllMotionStates;
-	bool	m_applySpeculativeContactRestitution;
+	bool m_ownsIslandManager;
+	bool m_ownsConstraintSolver;
+	bool m_synchronizeAllMotionStates;
+	bool m_applySpeculativeContactRestitution;
 
-	btAlignedObjectArray<btActionInterface*>	m_actions;
-	
-	int	m_profileTimings;
+	btAlignedObjectArray<btActionInterface*> m_actions;
 
-	bool	m_latencyMotionStateInterpolation;
+	int m_profileTimings;
 
-	btAlignedObjectArray<btPersistentManifold*>	m_predictiveManifolds;
+	bool m_latencyMotionStateInterpolation;
 
-	virtual void	predictUnconstraintMotion(btScalar timeStep);
-	
-	virtual void	integrateTransforms(btScalar timeStep);
-		
-	virtual void	calculateSimulationIslands();
+	btAlignedObjectArray<btPersistentManifold*> m_predictiveManifolds;
+	btSpinMutex m_predictiveManifoldsMutex;  // used to synchronize threads creating predictive contacts
 
-	virtual void	solveConstraints(btContactSolverInfo& solverInfo);
-	
-	virtual void	updateActivationState(btScalar timeStep);
+	virtual void predictUnconstraintMotion(btScalar timeStep);
+
+	void integrateTransformsInternal(btRigidBody * *bodies, int numBodies, btScalar timeStep);  // can be called in parallel
+	virtual void integrateTransforms(btScalar timeStep);
 
-	void	updateActions(btScalar timeStep);
+	virtual void calculateSimulationIslands();
 
-	void	startProfiling(btScalar timeStep);
+	
 
-	virtual void	internalSingleStepSimulation( btScalar timeStep);
+	virtual void updateActivationState(btScalar timeStep);
 
-	void	createPredictiveContacts(btScalar timeStep);
+	void updateActions(btScalar timeStep);
 
-	virtual void	saveKinematicState(btScalar timeStep);
+	void startProfiling(btScalar timeStep);
 
-	void	serializeRigidBodies(btSerializer* serializer);
+	virtual void internalSingleStepSimulation(btScalar timeStep);
 
-	void	serializeDynamicsWorldInfo(btSerializer* serializer);
+	void releasePredictiveContacts();
+	void createPredictiveContactsInternal(btRigidBody * *bodies, int numBodies, btScalar timeStep);  // can be called in parallel
+	virtual void createPredictiveContacts(btScalar timeStep);
 
-public:
+	virtual void saveKinematicState(btScalar timeStep);
 
+	void serializeRigidBodies(btSerializer * serializer);
 
+	void serializeDynamicsWorldInfo(btSerializer * serializer);
+    
+public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those
-	btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+	btDiscreteDynamicsWorld(btDispatcher * dispatcher, btBroadphaseInterface * pairCache, btConstraintSolver * constraintSolver, btCollisionConfiguration * collisionConfiguration);
 
 	virtual ~btDiscreteDynamicsWorld();
 
 	///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
-	virtual int	stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
+	virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
 
-
-	virtual void	synchronizeMotionStates();
+    virtual void solveConstraints(btContactSolverInfo & solverInfo);
+    
+	virtual void synchronizeMotionStates();
 
 	///this can be useful to synchronize a single rigid body -> graphics object
-	void	synchronizeSingleMotionState(btRigidBody* body);
+	void synchronizeSingleMotionState(btRigidBody * body);
 
-	virtual void	addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false);
+	virtual void addConstraint(btTypedConstraint * constraint, bool disableCollisionsBetweenLinkedBodies = false);
 
-	virtual void	removeConstraint(btTypedConstraint* constraint);
+	virtual void removeConstraint(btTypedConstraint * constraint);
 
-	virtual void	addAction(btActionInterface*);
+	virtual void addAction(btActionInterface*);
 
-	virtual void	removeAction(btActionInterface*);
-	
-	btSimulationIslandManager*	getSimulationIslandManager()
+	virtual void removeAction(btActionInterface*);
+
+	btSimulationIslandManager* getSimulationIslandManager()
 	{
 		return m_islandManager;
 	}
 
-	const btSimulationIslandManager*	getSimulationIslandManager() const 
+	const btSimulationIslandManager* getSimulationIslandManager() const
 	{
 		return m_islandManager;
 	}
 
-	btCollisionWorld*	getCollisionWorld()
+	btCollisionWorld* getCollisionWorld()
 	{
 		return this;
 	}
 
-	virtual void	setGravity(const btVector3& gravity);
+	virtual void setGravity(const btVector3& gravity);
 
-	virtual btVector3 getGravity () const;
+	virtual btVector3 getGravity() const;
 
-	virtual void	addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::StaticFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+	virtual void addCollisionObject(btCollisionObject * collisionObject, int collisionFilterGroup = btBroadphaseProxy::StaticFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
 
-	virtual void	addRigidBody(btRigidBody* body);
+	virtual void addRigidBody(btRigidBody * body);
 
-	virtual void	addRigidBody(btRigidBody* body, short group, short mask);
+	virtual void addRigidBody(btRigidBody * body, int group, int mask);
 
-	virtual void	removeRigidBody(btRigidBody* body);
+	virtual void removeRigidBody(btRigidBody * body);
 
 	///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
-	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
+	virtual void removeCollisionObject(btCollisionObject * collisionObject);
 
+	virtual void debugDrawConstraint(btTypedConstraint * constraint);
 
-	virtual void	debugDrawConstraint(btTypedConstraint* constraint);
+	virtual void debugDrawWorld();
 
-	virtual void	debugDrawWorld();
-
-	virtual void	setConstraintSolver(btConstraintSolver* solver);
+	virtual void setConstraintSolver(btConstraintSolver * solver);
 
 	virtual btConstraintSolver* getConstraintSolver();
-	
-	virtual	int		getNumConstraints() const;
 
-	virtual btTypedConstraint* getConstraint(int index)	;
+	virtual int getNumConstraints() const;
+
+	virtual btTypedConstraint* getConstraint(int index);
 
 	virtual const btTypedConstraint* getConstraint(int index) const;
 
-	
-	virtual btDynamicsWorldType	getWorldType() const
+	virtual btDynamicsWorldType getWorldType() const
 	{
 		return BT_DISCRETE_DYNAMICS_WORLD;
 	}
-	
+
 	///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
-	virtual void	clearForces();
+	virtual void clearForces();
 
 	///apply gravity, call this once per timestep
-	virtual void	applyGravity();
+	virtual void applyGravity();
 
-	virtual void	setNumTasks(int numTasks)
+	virtual void setNumTasks(int numTasks)
 	{
-        (void) numTasks;
+		(void)numTasks;
 	}
 
 	///obsolete, use updateActions instead
@@ -189,15 +189,15 @@ public:
 	}
 
 	///obsolete, use addAction instead
-	virtual void	addVehicle(btActionInterface* vehicle);
+	virtual void addVehicle(btActionInterface * vehicle);
 	///obsolete, use removeAction instead
-	virtual void	removeVehicle(btActionInterface* vehicle);
+	virtual void removeVehicle(btActionInterface * vehicle);
 	///obsolete, use addAction instead
-	virtual void	addCharacter(btActionInterface* character);
+	virtual void addCharacter(btActionInterface * character);
 	///obsolete, use removeAction instead
-	virtual void	removeCharacter(btActionInterface* character);
+	virtual void removeCharacter(btActionInterface * character);
 
-	void	setSynchronizeAllMotionStates(bool synchronizeAll)
+	void setSynchronizeAllMotionStates(bool synchronizeAll)
 	{
 		m_synchronizeAllMotionStates = synchronizeAll;
 	}
@@ -210,18 +210,18 @@ public:
 	{
 		m_applySpeculativeContactRestitution = enable;
 	}
-	
+
 	bool getApplySpeculativeContactRestitution() const
 	{
 		return m_applySpeculativeContactRestitution;
 	}
 
 	///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see Bullet/Demos/SerializeDemo)
-	virtual	void	serialize(btSerializer* serializer);
+	virtual void serialize(btSerializer * serializer);
 
 	///Interpolate motion state between previous and current transform, instead of current and next transform.
 	///This can relieve discontinuities in the rendering, due to penetrations
-	void setLatencyMotionStateInterpolation(bool latencyInterpolation )
+	void setLatencyMotionStateInterpolation(bool latencyInterpolation)
 	{
 		m_latencyMotionStateInterpolation = latencyInterpolation;
 	}
@@ -229,6 +229,16 @@ public:
 	{
 		return m_latencyMotionStateInterpolation;
 	}
+    
+    btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies()
+    {
+        return m_nonStaticRigidBodies;
+    }
+    
+    const btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies() const
+    {
+        return m_nonStaticRigidBodies;
+    }
 };
 
-#endif //BT_DISCRETE_DYNAMICS_WORLD_H
+#endif  //BT_DISCRETE_DYNAMICS_WORLD_H
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp
new file mode 100644
index 00000000000..8207b471356
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp
@@ -0,0 +1,263 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btDiscreteDynamicsWorldMt.h"
+
+//collision detection
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "btSimulationIslandManagerMt.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btQuickprof.h"
+
+//rigidbody & constraints
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
+
+#include "LinearMath/btIDebugDraw.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+#include "BulletDynamics/Dynamics/btActionInterface.h"
+#include "LinearMath/btQuickprof.h"
+#include "LinearMath/btMotionState.h"
+
+#include "LinearMath/btSerializer.h"
+
+///
+/// btConstraintSolverPoolMt
+///
+
+btConstraintSolverPoolMt::ThreadSolver* btConstraintSolverPoolMt::getAndLockThreadSolver()
+{
+	int i = 0;
+#if BT_THREADSAFE
+	i = btGetCurrentThreadIndex() % m_solvers.size();
+#endif  // #if BT_THREADSAFE
+	while (true)
+	{
+		ThreadSolver& solver = m_solvers[i];
+		if (solver.mutex.tryLock())
+		{
+			return &solver;
+		}
+		// failed, try the next one
+		i = (i + 1) % m_solvers.size();
+	}
+	return NULL;
+}
+
+void btConstraintSolverPoolMt::init(btConstraintSolver** solvers, int numSolvers)
+{
+	m_solverType = BT_SEQUENTIAL_IMPULSE_SOLVER;
+	m_solvers.resize(numSolvers);
+	for (int i = 0; i < numSolvers; ++i)
+	{
+		m_solvers[i].solver = solvers[i];
+	}
+	if (numSolvers > 0)
+	{
+		m_solverType = solvers[0]->getSolverType();
+	}
+}
+
+// create the solvers for me
+btConstraintSolverPoolMt::btConstraintSolverPoolMt(int numSolvers)
+{
+	btAlignedObjectArray<btConstraintSolver*> solvers;
+	solvers.reserve(numSolvers);
+	for (int i = 0; i < numSolvers; ++i)
+	{
+		btConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
+		solvers.push_back(solver);
+	}
+	init(&solvers[0], numSolvers);
+}
+
+// pass in fully constructed solvers (destructor will delete them)
+btConstraintSolverPoolMt::btConstraintSolverPoolMt(btConstraintSolver** solvers, int numSolvers)
+{
+	init(solvers, numSolvers);
+}
+
+btConstraintSolverPoolMt::~btConstraintSolverPoolMt()
+{
+	// delete all solvers
+	for (int i = 0; i < m_solvers.size(); ++i)
+	{
+		ThreadSolver& solver = m_solvers[i];
+		delete solver.solver;
+		solver.solver = NULL;
+	}
+}
+
+///solve a group of constraints
+btScalar btConstraintSolverPoolMt::solveGroup(btCollisionObject** bodies,
+											  int numBodies,
+											  btPersistentManifold** manifolds,
+											  int numManifolds,
+											  btTypedConstraint** constraints,
+											  int numConstraints,
+											  const btContactSolverInfo& info,
+											  btIDebugDraw* debugDrawer,
+											  btDispatcher* dispatcher)
+{
+	ThreadSolver* ts = getAndLockThreadSolver();
+	ts->solver->solveGroup(bodies, numBodies, manifolds, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
+	ts->mutex.unlock();
+	return 0.0f;
+}
+
+void btConstraintSolverPoolMt::reset()
+{
+	for (int i = 0; i < m_solvers.size(); ++i)
+	{
+		ThreadSolver& solver = m_solvers[i];
+		solver.mutex.lock();
+		solver.solver->reset();
+		solver.mutex.unlock();
+	}
+}
+
+///
+/// btDiscreteDynamicsWorldMt
+///
+
+btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher,
+													 btBroadphaseInterface* pairCache,
+													 btConstraintSolverPoolMt* solverPool,
+													 btConstraintSolver* constraintSolverMt,
+													 btCollisionConfiguration* collisionConfiguration)
+	: btDiscreteDynamicsWorld(dispatcher, pairCache, solverPool, collisionConfiguration)
+{
+	if (m_ownsIslandManager)
+	{
+		m_islandManager->~btSimulationIslandManager();
+		btAlignedFree(m_islandManager);
+	}
+	{
+		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt), 16);
+		btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt();
+		im->setMinimumSolverBatchSize(m_solverInfo.m_minimumSolverBatchSize);
+		m_islandManager = im;
+	}
+	m_constraintSolverMt = constraintSolverMt;
+}
+
+btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt()
+{
+}
+
+void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo)
+{
+	BT_PROFILE("solveConstraints");
+
+	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
+
+	/// solve all the constraints for this island
+	btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager);
+	btSimulationIslandManagerMt::SolverParams solverParams;
+	solverParams.m_solverPool = m_constraintSolver;
+	solverParams.m_solverMt = m_constraintSolverMt;
+	solverParams.m_solverInfo = &solverInfo;
+	solverParams.m_debugDrawer = m_debugDrawer;
+	solverParams.m_dispatcher = getCollisionWorld()->getDispatcher();
+	im->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, solverParams);
+
+	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
+}
+
+struct UpdaterUnconstrainedMotion : public btIParallelForBody
+{
+	btScalar timeStep;
+	btRigidBody** rigidBodies;
+
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		for (int i = iBegin; i < iEnd; ++i)
+		{
+			btRigidBody* body = rigidBodies[i];
+			if (!body->isStaticOrKinematicObject())
+			{
+				//don't integrate/update velocities here, it happens in the constraint solver
+				body->applyDamping(timeStep);
+				body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform());
+			}
+		}
+	}
+};
+
+void btDiscreteDynamicsWorldMt::predictUnconstraintMotion(btScalar timeStep)
+{
+	BT_PROFILE("predictUnconstraintMotion");
+	if (m_nonStaticRigidBodies.size() > 0)
+	{
+		UpdaterUnconstrainedMotion update;
+		update.timeStep = timeStep;
+		update.rigidBodies = &m_nonStaticRigidBodies[0];
+		int grainSize = 50;  // num of iterations per task for task scheduler
+		btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update);
+	}
+}
+
+void btDiscreteDynamicsWorldMt::createPredictiveContacts(btScalar timeStep)
+{
+	BT_PROFILE("createPredictiveContacts");
+	releasePredictiveContacts();
+	if (m_nonStaticRigidBodies.size() > 0)
+	{
+		UpdaterCreatePredictiveContacts update;
+		update.world = this;
+		update.timeStep = timeStep;
+		update.rigidBodies = &m_nonStaticRigidBodies[0];
+		int grainSize = 50;  // num of iterations per task for task scheduler
+		btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update);
+	}
+}
+
+void btDiscreteDynamicsWorldMt::integrateTransforms(btScalar timeStep)
+{
+	BT_PROFILE("integrateTransforms");
+	if (m_nonStaticRigidBodies.size() > 0)
+	{
+		UpdaterIntegrateTransforms update;
+		update.world = this;
+		update.timeStep = timeStep;
+		update.rigidBodies = &m_nonStaticRigidBodies[0];
+		int grainSize = 50;  // num of iterations per task for task scheduler
+		btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update);
+	}
+}
+
+int btDiscreteDynamicsWorldMt::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
+{
+	int numSubSteps = btDiscreteDynamicsWorld::stepSimulation(timeStep, maxSubSteps, fixedTimeStep);
+	if (btITaskScheduler* scheduler = btGetTaskScheduler())
+	{
+		// tell Bullet's threads to sleep, so other threads can run
+		scheduler->sleepWorkerThreadsHint();
+	}
+	return numSubSteps;
+}
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h
new file mode 100644
index 00000000000..dccf35d7a7a
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h
@@ -0,0 +1,131 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_DISCRETE_DYNAMICS_WORLD_MT_H
+#define BT_DISCRETE_DYNAMICS_WORLD_MT_H
+
+#include "btDiscreteDynamicsWorld.h"
+#include "btSimulationIslandManagerMt.h"
+#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
+
+///
+/// btConstraintSolverPoolMt - masquerades as a constraint solver, but really it is a threadsafe pool of them.
+///
+///  Each solver in the pool is protected by a mutex.  When solveGroup is called from a thread,
+///  the pool looks for a solver that isn't being used by another thread, locks it, and dispatches the
+///  call to the solver.
+///  So long as there are at least as many solvers as there are hardware threads, it should never need to
+///  spin wait.
+///
+class btConstraintSolverPoolMt : public btConstraintSolver
+{
+public:
+	// create the solvers for me
+	explicit btConstraintSolverPoolMt(int numSolvers);
+
+	// pass in fully constructed solvers (destructor will delete them)
+	btConstraintSolverPoolMt(btConstraintSolver** solvers, int numSolvers);
+
+	virtual ~btConstraintSolverPoolMt();
+
+	///solve a group of constraints
+	virtual btScalar solveGroup(btCollisionObject** bodies,
+								int numBodies,
+								btPersistentManifold** manifolds,
+								int numManifolds,
+								btTypedConstraint** constraints,
+								int numConstraints,
+								const btContactSolverInfo& info,
+								btIDebugDraw* debugDrawer,
+								btDispatcher* dispatcher) BT_OVERRIDE;
+
+	virtual void reset() BT_OVERRIDE;
+	virtual btConstraintSolverType getSolverType() const BT_OVERRIDE { return m_solverType; }
+
+private:
+	const static size_t kCacheLineSize = 128;
+	struct ThreadSolver
+	{
+		btConstraintSolver* solver;
+		btSpinMutex mutex;
+		char _cachelinePadding[kCacheLineSize - sizeof(btSpinMutex) - sizeof(void*)];  // keep mutexes from sharing a cache line
+	};
+	btAlignedObjectArray<ThreadSolver> m_solvers;
+	btConstraintSolverType m_solverType;
+
+	ThreadSolver* getAndLockThreadSolver();
+	void init(btConstraintSolver** solvers, int numSolvers);
+};
+
+///
+/// btDiscreteDynamicsWorldMt -- a version of DiscreteDynamicsWorld with some minor changes to support
+///                              solving simulation islands on multiple threads.
+///
+///  Should function exactly like btDiscreteDynamicsWorld.
+///  Also 3 methods that iterate over all of the rigidbodies can run in parallel:
+///     - predictUnconstraintMotion
+///     - integrateTransforms
+///     - createPredictiveContacts
+///
+ATTRIBUTE_ALIGNED16(class)
+btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld
+{
+protected:
+	btConstraintSolver* m_constraintSolverMt;
+
+	virtual void solveConstraints(btContactSolverInfo & solverInfo) BT_OVERRIDE;
+
+	virtual void predictUnconstraintMotion(btScalar timeStep) BT_OVERRIDE;
+
+	struct UpdaterCreatePredictiveContacts : public btIParallelForBody
+	{
+		btScalar timeStep;
+		btRigidBody** rigidBodies;
+		btDiscreteDynamicsWorldMt* world;
+
+		void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+		{
+			world->createPredictiveContactsInternal(&rigidBodies[iBegin], iEnd - iBegin, timeStep);
+		}
+	};
+	virtual void createPredictiveContacts(btScalar timeStep) BT_OVERRIDE;
+
+	struct UpdaterIntegrateTransforms : public btIParallelForBody
+	{
+		btScalar timeStep;
+		btRigidBody** rigidBodies;
+		btDiscreteDynamicsWorldMt* world;
+
+		void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+		{
+			world->integrateTransformsInternal(&rigidBodies[iBegin], iEnd - iBegin, timeStep);
+		}
+	};
+	virtual void integrateTransforms(btScalar timeStep) BT_OVERRIDE;
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btDiscreteDynamicsWorldMt(btDispatcher * dispatcher,
+							  btBroadphaseInterface * pairCache,
+							  btConstraintSolverPoolMt * solverPool,        // Note this should be a solver-pool for multi-threading
+							  btConstraintSolver * constraintSolverMt,      // single multi-threaded solver for large islands (or NULL)
+							  btCollisionConfiguration * collisionConfiguration);
+	virtual ~btDiscreteDynamicsWorldMt();
+
+	virtual int stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) BT_OVERRIDE;
+};
+
+#endif  //BT_DISCRETE_DYNAMICS_WORLD_H
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
index 35dd1400fe7..3c55234a8a9 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
@@ -24,144 +24,151 @@ class btActionInterface;
 class btConstraintSolver;
 class btDynamicsWorld;
 
-
 /// Type for the callback for each tick
-typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep);
+typedef void (*btInternalTickCallback)(btDynamicsWorld* world, btScalar timeStep);
 
 enum btDynamicsWorldType
 {
-	BT_SIMPLE_DYNAMICS_WORLD=1,
-	BT_DISCRETE_DYNAMICS_WORLD=2,
-	BT_CONTINUOUS_DYNAMICS_WORLD=3,
-	BT_SOFT_RIGID_DYNAMICS_WORLD=4,
-	BT_GPU_DYNAMICS_WORLD=5
+	BT_SIMPLE_DYNAMICS_WORLD = 1,
+	BT_DISCRETE_DYNAMICS_WORLD = 2,
+	BT_CONTINUOUS_DYNAMICS_WORLD = 3,
+	BT_SOFT_RIGID_DYNAMICS_WORLD = 4,
+	BT_GPU_DYNAMICS_WORLD = 5,
+	BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6,
+    BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD = 7
 };
 
 ///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.
 class btDynamicsWorld : public btCollisionWorld
 {
-
 protected:
-		btInternalTickCallback m_internalTickCallback;
-		btInternalTickCallback m_internalPreTickCallback;
-		void*	m_worldUserInfo;
+	btInternalTickCallback m_internalTickCallback;
+	btInternalTickCallback m_internalPreTickCallback;
+	void* m_worldUserInfo;
 
-		btContactSolverInfo	m_solverInfo;
+	btContactSolverInfo m_solverInfo;
 
 public:
-		
+	btDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* broadphase, btCollisionConfiguration* collisionConfiguration)
+		: btCollisionWorld(dispatcher, broadphase, collisionConfiguration), m_internalTickCallback(0), m_internalPreTickCallback(0), m_worldUserInfo(0)
+	{
+	}
 
-		btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration)
-		:btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0),m_internalPreTickCallback(0), m_worldUserInfo(0)
-		{
-		}
+	virtual ~btDynamicsWorld()
+	{
+	}
 
-		virtual ~btDynamicsWorld()
-		{
-		}
-		
-		///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds.
-		///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'.
-		///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'.
-		///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant.
-		virtual int		stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0;
-			
-		virtual void	debugDrawWorld() = 0;
-				
-		virtual void	addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) 
-		{ 
-			(void)constraint; (void)disableCollisionsBetweenLinkedBodies;
-		}
+	///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds.
+	///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'.
+	///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'.
+	///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant.
+	virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)) = 0;
 
-		virtual void	removeConstraint(btTypedConstraint* constraint) {(void)constraint;}
+	virtual void debugDrawWorld() = 0;
 
-		virtual void	addAction(btActionInterface* action) = 0;
+	virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies = false)
+	{
+		(void)constraint;
+		(void)disableCollisionsBetweenLinkedBodies;
+	}
 
-		virtual void	removeAction(btActionInterface* action) = 0;
+	virtual void removeConstraint(btTypedConstraint* constraint) { (void)constraint; }
 
-		//once a rigidbody is added to the dynamics world, it will get this gravity assigned
-		//existing rigidbodies in the world get gravity assigned too, during this method
-		virtual void	setGravity(const btVector3& gravity) = 0;
-		virtual btVector3 getGravity () const = 0;
+	virtual void addAction(btActionInterface* action) = 0;
 
-		virtual void	synchronizeMotionStates() = 0;
+	virtual void removeAction(btActionInterface* action) = 0;
 
-		virtual void	addRigidBody(btRigidBody* body) = 0;
+	//once a rigidbody is added to the dynamics world, it will get this gravity assigned
+	//existing rigidbodies in the world get gravity assigned too, during this method
+	virtual void setGravity(const btVector3& gravity) = 0;
+	virtual btVector3 getGravity() const = 0;
 
-		virtual void	addRigidBody(btRigidBody* body, short group, short mask) = 0;
+	virtual void synchronizeMotionStates() = 0;
 
-		virtual void	removeRigidBody(btRigidBody* body) = 0;
+	virtual void addRigidBody(btRigidBody* body) = 0;
 
-		virtual void	setConstraintSolver(btConstraintSolver* solver) = 0;
+	virtual void addRigidBody(btRigidBody* body, int group, int mask) = 0;
 
-		virtual btConstraintSolver* getConstraintSolver() = 0;
-		
-		virtual	int		getNumConstraints() const {	return 0;		}
-		
-		virtual btTypedConstraint* getConstraint(int index)		{	(void)index;		return 0;		}
-		
-		virtual const btTypedConstraint* getConstraint(int index) const	{	(void)index;	return 0;	}
+	virtual void removeRigidBody(btRigidBody* body) = 0;
 
-		virtual btDynamicsWorldType	getWorldType() const=0;
+	virtual void setConstraintSolver(btConstraintSolver* solver) = 0;
 
-		virtual void	clearForces() = 0;
+	virtual btConstraintSolver* getConstraintSolver() = 0;
 
-		/// Set the callback for when an internal tick (simulation substep) happens, optional user info
-		void setInternalTickCallback(btInternalTickCallback cb,	void* worldUserInfo=0,bool isPreTick=false) 
-		{ 
-			if (isPreTick)
-			{
-				m_internalPreTickCallback = cb;
-			} else
-			{
-				m_internalTickCallback = cb; 
-			}
-			m_worldUserInfo = worldUserInfo;
-		}
+	virtual int getNumConstraints() const { return 0; }
 
-		void	setWorldUserInfo(void* worldUserInfo)
-		{
-			m_worldUserInfo = worldUserInfo;
-		}
+	virtual btTypedConstraint* getConstraint(int index)
+	{
+		(void)index;
+		return 0;
+	}
+
+	virtual const btTypedConstraint* getConstraint(int index) const
+	{
+		(void)index;
+		return 0;
+	}
 
-		void*	getWorldUserInfo() const
+	virtual btDynamicsWorldType getWorldType() const = 0;
+
+	virtual void clearForces() = 0;
+
+	/// Set the callback for when an internal tick (simulation substep) happens, optional user info
+	void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo = 0, bool isPreTick = false)
+	{
+		if (isPreTick)
 		{
-			return m_worldUserInfo;
+			m_internalPreTickCallback = cb;
 		}
-
-		btContactSolverInfo& getSolverInfo()
+		else
 		{
-			return m_solverInfo;
+			m_internalTickCallback = cb;
 		}
-
-
-		///obsolete, use addAction instead.
-		virtual void	addVehicle(btActionInterface* vehicle) {(void)vehicle;}
-		///obsolete, use removeAction instead
-		virtual void	removeVehicle(btActionInterface* vehicle) {(void)vehicle;}
-		///obsolete, use addAction instead.
-		virtual void	addCharacter(btActionInterface* character) {(void)character;}
-		///obsolete, use removeAction instead
-		virtual void	removeCharacter(btActionInterface* character) {(void)character;}
-
-
+		m_worldUserInfo = worldUserInfo;
+	}
+
+	void setWorldUserInfo(void* worldUserInfo)
+	{
+		m_worldUserInfo = worldUserInfo;
+	}
+
+	void* getWorldUserInfo() const
+	{
+		return m_worldUserInfo;
+	}
+
+	btContactSolverInfo& getSolverInfo()
+	{
+		return m_solverInfo;
+	}
+
+	const btContactSolverInfo& getSolverInfo() const
+	{
+		return m_solverInfo;
+	}
+
+	///obsolete, use addAction instead.
+	virtual void addVehicle(btActionInterface* vehicle) { (void)vehicle; }
+	///obsolete, use removeAction instead
+	virtual void removeVehicle(btActionInterface* vehicle) { (void)vehicle; }
+	///obsolete, use addAction instead.
+	virtual void addCharacter(btActionInterface* character) { (void)character; }
+	///obsolete, use removeAction instead
+	virtual void removeCharacter(btActionInterface* character) { (void)character; }
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct btDynamicsWorldDoubleData
 {
-	btContactSolverInfoDoubleData	m_solverInfo;
-	btVector3DoubleData	m_gravity;
+	btContactSolverInfoDoubleData m_solverInfo;
+	btVector3DoubleData m_gravity;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct btDynamicsWorldFloatData
 {
-	btContactSolverInfoFloatData	m_solverInfo;
-	btVector3FloatData	m_gravity;
+	btContactSolverInfoFloatData m_solverInfo;
+	btVector3FloatData m_gravity;
 };
 
-
-#endif //BT_DYNAMICS_WORLD_H
-
-
+#endif  //BT_DYNAMICS_WORLD_H
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
index a7882684bf1..27fdead7617 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
@@ -22,36 +22,34 @@ subject to the following restrictions:
 #include "LinearMath/btSerializer.h"
 
 //'temporarily' global variables
-btScalar	gDeactivationTime = btScalar(2.);
-bool	gDisableDeactivation = false;
+btScalar gDeactivationTime = btScalar(2.);
+bool gDisableDeactivation = false;
 static int uniqueId = 0;
 
-
 btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
 {
 	setupRigidBody(constructionInfo);
 }
 
-btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia)
+btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia)
 {
-	btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia);
+	btRigidBodyConstructionInfo cinfo(mass, motionState, collisionShape, localInertia);
 	setupRigidBody(cinfo);
 }
 
-void	btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
+void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
 {
-
-	m_internalType=CO_RIGID_BODY;
+	m_internalType = CO_RIGID_BODY;
 
 	m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
-	m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
-	m_angularFactor.setValue(1,1,1);
-	m_linearFactor.setValue(1,1,1);
+	m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+	m_angularFactor.setValue(1, 1, 1);
+	m_linearFactor.setValue(1, 1, 1);
 	m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 	m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 	m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 	m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-    setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
+		setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
 
 	m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
 	m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
@@ -67,46 +65,44 @@ void	btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
 	if (m_optionalMotionState)
 	{
 		m_optionalMotionState->getWorldTransform(m_worldTransform);
-	} else
+	}
+	else
 	{
 		m_worldTransform = constructionInfo.m_startWorldTransform;
 	}
 
 	m_interpolationWorldTransform = m_worldTransform;
-	m_interpolationLinearVelocity.setValue(0,0,0);
-	m_interpolationAngularVelocity.setValue(0,0,0);
-	
+	m_interpolationLinearVelocity.setValue(0, 0, 0);
+	m_interpolationAngularVelocity.setValue(0, 0, 0);
+
 	//moved to btCollisionObject
 	m_friction = constructionInfo.m_friction;
 	m_rollingFriction = constructionInfo.m_rollingFriction;
+	m_spinningFriction = constructionInfo.m_spinningFriction;
+
 	m_restitution = constructionInfo.m_restitution;
 
-	setCollisionShape( constructionInfo.m_collisionShape );
+	setCollisionShape(constructionInfo.m_collisionShape);
 	m_debugBodyId = uniqueId++;
-	
+
 	setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
 	updateInertiaTensor();
 
 	m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY;
 
-
 	m_deltaLinearVelocity.setZero();
 	m_deltaAngularVelocity.setZero();
-	m_invMass = m_inverseMass*m_linearFactor;
+	m_invMass = m_inverseMass * m_linearFactor;
 	m_pushVelocity.setZero();
 	m_turnVelocity.setZero();
-
-	
-
 }
 
-
-void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
+void btRigidBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform)
 {
-	btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
+	btTransformUtil::integrateTransform(m_worldTransform, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform);
 }
 
-void			btRigidBody::saveKinematicState(btScalar timeStep)
+void btRigidBody::saveKinematicState(btScalar timeStep)
 {
 	//todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
 	if (timeStep != btScalar(0.))
@@ -114,25 +110,22 @@ void			btRigidBody::saveKinematicState(btScalar timeStep)
 		//if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
 		if (getMotionState())
 			getMotionState()->getWorldTransform(m_worldTransform);
-		btVector3 linVel,angVel;
-		
-		btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
+		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());
 	}
 }
-	
-void	btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
+
+void btRigidBody::getAabb(btVector3& aabbMin, btVector3& aabbMax) const
 {
-	getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax);
+	getCollisionShape()->getAabb(m_worldTransform, aabbMin, aabbMax);
 }
 
-
-
-
-void btRigidBody::setGravity(const btVector3& acceleration) 
+void btRigidBody::setGravity(const btVector3& acceleration)
 {
 	if (m_inverseMass != btScalar(0.0))
 	{
@@ -141,33 +134,29 @@ void btRigidBody::setGravity(const btVector3& acceleration)
 	m_gravity_acceleration = acceleration;
 }
 
-
-
-
-
-
 void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
 {
-	m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
-	m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+#ifdef BT_USE_OLD_DAMPING_METHOD
+	m_linearDamping = btMax(lin_damping, btScalar(0.0));
+	m_angularDamping = btMax(ang_damping, btScalar(0.0));
+#else
+	m_linearDamping = btClamped(lin_damping, btScalar(0.0), btScalar(1.0));
+	m_angularDamping = btClamped(ang_damping, btScalar(0.0), btScalar(1.0));
+#endif
 }
 
-
-
-
 ///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
-void			btRigidBody::applyDamping(btScalar timeStep)
+void btRigidBody::applyDamping(btScalar timeStep)
 {
 	//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
 	//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
 
-//#define USE_OLD_DAMPING_METHOD 1
-#ifdef USE_OLD_DAMPING_METHOD
-	m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
-	m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+#ifdef BT_USE_OLD_DAMPING_METHOD
+	m_linearVelocity *= btMax((btScalar(1.0) - timeStep * m_linearDamping), btScalar(0.0));
+	m_angularVelocity *= btMax((btScalar(1.0) - timeStep * m_angularDamping), btScalar(0.0));
 #else
-	m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep);
-	m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep);
+	m_linearVelocity *= btPow(btScalar(1) - m_linearDamping, timeStep);
+	m_angularVelocity *= btPow(btScalar(1) - m_angularDamping, timeStep);
 #endif
 
 	if (m_additionalDamping)
@@ -180,7 +169,6 @@ void			btRigidBody::applyDamping(btScalar timeStep)
 			m_angularVelocity *= m_additionalDampingFactor;
 			m_linearVelocity *= m_additionalDampingFactor;
 		}
-	
 
 		btScalar speed = m_linearVelocity.length();
 		if (speed < m_linearDamping)
@@ -189,10 +177,11 @@ void			btRigidBody::applyDamping(btScalar timeStep)
 			if (speed > dampVel)
 			{
 				btVector3 dir = m_linearVelocity.normalized();
-				m_linearVelocity -=  dir * dampVel;
-			} else
+				m_linearVelocity -= dir * dampVel;
+			}
+			else
 			{
-				m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+				m_linearVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 			}
 		}
 
@@ -203,30 +192,36 @@ void			btRigidBody::applyDamping(btScalar timeStep)
 			if (angSpeed > angDampVel)
 			{
 				btVector3 dir = m_angularVelocity.normalized();
-				m_angularVelocity -=  dir * angDampVel;
-			} else
+				m_angularVelocity -= dir * angDampVel;
+			}
+			else
 			{
-				m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+				m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 			}
 		}
 	}
 }
 
-
 void btRigidBody::applyGravity()
 {
 	if (isStaticOrKinematicObject())
 		return;
-	
-	applyCentralForce(m_gravity);	
 
+	applyCentralForce(m_gravity);
+}
+
+void btRigidBody::clearGravity()
+{
+    if (isStaticOrKinematicObject())
+        return;
+    
+    applyCentralForce(-m_gravity);
 }
 
 void btRigidBody::proceedToTransform(const btTransform& newTrans)
 {
-	setCenterOfMassTransform( newTrans );
+	setCenterOfMassTransform(newTrans);
 }
-	
 
 void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
 {
@@ -234,7 +229,8 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
 	{
 		m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
 		m_inverseMass = btScalar(0.);
-	} else
+	}
+	else
 	{
 		m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
 		m_inverseMass = btScalar(1.0) / mass;
@@ -242,50 +238,45 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
 
 	//Fg = m * a
 	m_gravity = mass * m_gravity_acceleration;
-	
-	m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
-				   inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
-				   inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
 
-	m_invMass = m_linearFactor*m_inverseMass;
+	m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
+							   inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
+							   inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
+
+	m_invMass = m_linearFactor * m_inverseMass;
 }
 
-	
-void btRigidBody::updateInertiaTensor() 
+void btRigidBody::updateInertiaTensor()
 {
 	m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
 }
 
-
-
 btVector3 btRigidBody::getLocalInertia() const
 {
-
 	btVector3 inertiaLocal;
 	const btVector3 inertia = m_invInertiaLocal;
 	inertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
-		inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
-		inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
+						  inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
+						  inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
 	return inertiaLocal;
 }
 
 inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt,
-	const btMatrix3x3 &I)
+							  const btMatrix3x3& I)
 {
-	const btVector3 w2 = I*w1 + w1.cross(I*w1)*dt - (T*dt + I*w0);
+	const btVector3 w2 = I * w1 + w1.cross(I * w1) * dt - (T * dt + I * w0);
 	return w2;
 }
 
 inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, const btScalar dt,
-	const btMatrix3x3 &I)
+									 const btMatrix3x3& I)
 {
-
 	btMatrix3x3 w1x, Iw1x;
-	const btVector3 Iwi = (I*w1);
+	const btVector3 Iwi = (I * w1);
 	w1.getSkewSymmetricMatrix(&w1x[0], &w1x[1], &w1x[2]);
 	Iwi.getSkewSymmetricMatrix(&Iw1x[0], &Iw1x[1], &Iw1x[2]);
 
-	const btMatrix3x3 dfw1 = I + (w1x*I - Iw1x)*dt;
+	const btMatrix3x3 dfw1 = I + (w1x * I - Iw1x) * dt;
 	return dfw1;
 }
 
@@ -293,58 +284,55 @@ btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForc
 {
 	btVector3 inertiaLocal = getLocalInertia();
 	btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
-	btVector3 tmp = inertiaTensorWorld*getAngularVelocity();
+	btVector3 tmp = inertiaTensorWorld * getAngularVelocity();
 	btVector3 gf = getAngularVelocity().cross(tmp);
 	btScalar l2 = gf.length2();
-	if (l2>maxGyroscopicForce*maxGyroscopicForce)
+	if (l2 > maxGyroscopicForce * maxGyroscopicForce)
 	{
-		gf *= btScalar(1.)/btSqrt(l2)*maxGyroscopicForce;
+		gf *= btScalar(1.) / btSqrt(l2) * maxGyroscopicForce;
 	}
 	return gf;
 }
 
-
 btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(btScalar step) const
-{	
+{
 	btVector3 idl = getLocalInertia();
 	btVector3 omega1 = getAngularVelocity();
 	btQuaternion q = getWorldTransform().getRotation();
-	
+
 	// Convert to body coordinates
 	btVector3 omegab = quatRotate(q.inverse(), omega1);
 	btMatrix3x3 Ib;
-	Ib.setValue(idl.x(),0,0,
-				0,idl.y(),0,
-				0,0,idl.z());
-	
-	btVector3 ibo = Ib*omegab;
+	Ib.setValue(idl.x(), 0, 0,
+				0, idl.y(), 0,
+				0, 0, idl.z());
+
+	btVector3 ibo = Ib * omegab;
 
 	// Residual vector
 	btVector3 f = step * omegab.cross(ibo);
-	
+
 	btMatrix3x3 skew0;
 	omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]);
-	btVector3 om = Ib*omegab;
+	btVector3 om = Ib * omegab;
 	btMatrix3x3 skew1;
-	om.getSkewSymmetricMatrix(&skew1[0],&skew1[1],&skew1[2]);
-	
+	om.getSkewSymmetricMatrix(&skew1[0], &skew1[1], &skew1[2]);
+
 	// Jacobian
-	btMatrix3x3 J = Ib +  (skew0*Ib - skew1)*step;
-	
-//	btMatrix3x3 Jinv = J.inverse();
-//	btVector3 omega_div = Jinv*f;
+	btMatrix3x3 J = Ib + (skew0 * Ib - skew1) * step;
+
+	//	btMatrix3x3 Jinv = J.inverse();
+	//	btVector3 omega_div = Jinv*f;
 	btVector3 omega_div = J.solve33(f);
-	
+
 	// Single Newton-Raphson update
-	omegab = omegab - omega_div;//Solve33(J, f);
+	omegab = omegab - omega_div;  //Solve33(J, f);
 	// Back to world coordinates
-	btVector3 omega2 = quatRotate(q,omegab);
-	btVector3 gf = omega2-omega1;
+	btVector3 omega2 = quatRotate(q, omegab);
+	btVector3 gf = omega2 - omega1;
 	return gf;
 }
 
-
-
 btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) const
 {
 	// use full newton-euler equations.  common practice to drop the wxIw term. want it for better tumbling behavior.
@@ -359,7 +347,7 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) con
 		m_worldTransform.getBasis().transpose();
 
 	// use newtons method to find implicit solution for new angular velocity (w')
-	// f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 
+	// f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0
 	// df/dw' = I + 1xIw'*step + w'xI*step
 
 	btVector3 w1 = w0;
@@ -381,8 +369,7 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) con
 	return gf;
 }
 
-
-void btRigidBody::integrateVelocities(btScalar step) 
+void btRigidBody::integrateVelocities(btScalar step)
 {
 	if (isStaticOrKinematicObject())
 		return;
@@ -391,30 +378,31 @@ void btRigidBody::integrateVelocities(btScalar step)
 	m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
 
 #define MAX_ANGVEL SIMD_HALF_PI
-	/// clamp angular velocity. collision calculations will fail on higher angular velocities	
+	/// clamp angular velocity. collision calculations will fail on higher angular velocities
 	btScalar angvel = m_angularVelocity.length();
-	if (angvel*step > MAX_ANGVEL)
+	if (angvel * step > MAX_ANGVEL)
 	{
-		m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
+		m_angularVelocity *= (MAX_ANGVEL / step) / angvel;
 	}
-
+	#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+	clampVelocity(m_angularVelocity);
+	#endif
 }
 
 btQuaternion btRigidBody::getOrientation() const
 {
-		btQuaternion orn;
-		m_worldTransform.getBasis().getRotation(orn);
-		return orn;
+	btQuaternion orn;
+	m_worldTransform.getBasis().getRotation(orn);
+	return orn;
 }
-	
-	
+
 void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
 {
-
 	if (isKinematicObject())
 	{
 		m_interpolationWorldTransform = m_worldTransform;
-	} else
+	}
+	else
 	{
 		m_interpolationWorldTransform = xform;
 	}
@@ -424,51 +412,59 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
 	updateInertiaTensor();
 }
 
-
-bool btRigidBody::checkCollideWithOverride(const  btCollisionObject* co) const
-{
-	const btRigidBody* otherRb = btRigidBody::upcast(co);
-	if (!otherRb)
-		return true;
-
-	for (int i = 0; i < m_constraintRefs.size(); ++i)
-	{
-		const btTypedConstraint* c = m_constraintRefs[i];
-		if (c->isEnabled())
-			if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
-				return false;
-	}
-
-	return true;
-}
-
-
-
 void btRigidBody::addConstraintRef(btTypedConstraint* c)
 {
+	///disable collision with the 'other' body
+
 	int index = m_constraintRefs.findLinearSearch(c);
+	//don't add constraints that are already referenced
+	//btAssert(index == m_constraintRefs.size());
 	if (index == m_constraintRefs.size())
-		m_constraintRefs.push_back(c); 
-
-	m_checkCollideWith = true;
+	{
+		m_constraintRefs.push_back(c);
+		btCollisionObject* colObjA = &c->getRigidBodyA();
+		btCollisionObject* colObjB = &c->getRigidBodyB();
+		if (colObjA == this)
+		{
+			colObjA->setIgnoreCollisionCheck(colObjB, true);
+		}
+		else
+		{
+			colObjB->setIgnoreCollisionCheck(colObjA, true);
+		}
+	}
 }
 
 void btRigidBody::removeConstraintRef(btTypedConstraint* c)
 {
-	m_constraintRefs.remove(c);
-	m_checkCollideWith = m_constraintRefs.size() > 0;
+	int index = m_constraintRefs.findLinearSearch(c);
+	//don't remove constraints that are not referenced
+	if (index < m_constraintRefs.size())
+	{
+		m_constraintRefs.remove(c);
+		btCollisionObject* colObjA = &c->getRigidBodyA();
+		btCollisionObject* colObjB = &c->getRigidBodyB();
+		if (colObjA == this)
+		{
+			colObjA->setIgnoreCollisionCheck(colObjB, false);
+		}
+		else
+		{
+			colObjB->setIgnoreCollisionCheck(colObjA, false);
+		}
+	}
 }
 
-int	btRigidBody::calculateSerializeBufferSize()	const
+int btRigidBody::calculateSerializeBufferSize() const
 {
 	int sz = sizeof(btRigidBodyData);
 	return sz;
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-const char*	btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
 {
-	btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer;
+	btRigidBodyData* rbd = (btRigidBodyData*)dataBuffer;
 
 	btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer);
 
@@ -490,19 +486,20 @@ const char*	btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali
 	rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr;
 	rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr;
 	rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor;
-	rbd->m_linearSleepingThreshold=m_linearSleepingThreshold;
+	rbd->m_linearSleepingThreshold = m_linearSleepingThreshold;
 	rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
 
+	// Fill padding with zeros to appease msan.
+#ifdef BT_USE_DOUBLE_PRECISION
+	memset(rbd->m_padding, 0, sizeof(rbd->m_padding));
+#endif
+
 	return btRigidBodyDataName;
 }
 
-
-
 void btRigidBody::serializeSingleObject(class btSerializer* serializer) const
 {
-	btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1);
+	btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(), 1);
 	const char* structType = serialize(chunk->m_oldPtr, serializer);
-	serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this);
+	serializer->finalizeChunk(chunk, structType, BT_RIGIDBODY_CODE, (void*)this);
 }
-
-
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
index c2f8c5d64ae..7442dd1e6a6 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
@@ -25,206 +25,197 @@ class btCollisionShape;
 class btMotionState;
 class btTypedConstraint;
 
-
 extern btScalar gDeactivationTime;
 extern bool gDisableDeactivation;
 
 #ifdef BT_USE_DOUBLE_PRECISION
-#define btRigidBodyData	btRigidBodyDoubleData
-#define btRigidBodyDataName	"btRigidBodyDoubleData"
+#define btRigidBodyData btRigidBodyDoubleData
+#define btRigidBodyDataName "btRigidBodyDoubleData"
 #else
-#define btRigidBodyData	btRigidBodyFloatData
-#define btRigidBodyDataName	"btRigidBodyFloatData"
-#endif //BT_USE_DOUBLE_PRECISION
-
+#define btRigidBodyData btRigidBodyFloatData
+#define btRigidBodyDataName "btRigidBodyFloatData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
-enum	btRigidBodyFlags
+enum btRigidBodyFlags
 {
 	BT_DISABLE_WORLD_GRAVITY = 1,
 	///BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards.
 	///and it BT_ENABLE_GYROPSCOPIC_FORCE becomes equivalent to BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY
 	///See Demos/GyroscopicDemo and computeGyroscopicImpulseImplicit
 	BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT = 2,
-	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD=4,
-	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY=8,
+	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD = 4,
+	BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY = 8,
 	BT_ENABLE_GYROPSCOPIC_FORCE = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY,
 };
 
-
 ///The btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape.
 ///It is recommended for performance and memory use to share btCollisionShape objects whenever possible.
-///There are 3 types of rigid bodies: 
+///There are 3 types of rigid bodies:
 ///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics.
 ///- B) Fixed objects with zero mass. They are not moving (basically collision objects)
-///- C) Kinematic objects, which are objects without mass, but the user can move them. There is on-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform.
+///- C) Kinematic objects, which are objects without mass, but the user can move them. There is one-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform.
 ///Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time.
 ///Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects)
-class btRigidBody  : public btCollisionObject
+class btRigidBody : public btCollisionObject
 {
-
-	btMatrix3x3	m_invInertiaTensorWorld;
-	btVector3		m_linearVelocity;
-	btVector3		m_angularVelocity;
-	btScalar		m_inverseMass;
-	btVector3		m_linearFactor;
-
-	btVector3		m_gravity;	
-	btVector3		m_gravity_acceleration;
-	btVector3		m_invInertiaLocal;
-	btVector3		m_totalForce;
-	btVector3		m_totalTorque;
-	
-	btScalar		m_linearDamping;
-	btScalar		m_angularDamping;
-
-	bool			m_additionalDamping;
-	btScalar		m_additionalDampingFactor;
-	btScalar		m_additionalLinearDampingThresholdSqr;
-	btScalar		m_additionalAngularDampingThresholdSqr;
-	btScalar		m_additionalAngularDampingFactor;
-
-
-	btScalar		m_linearSleepingThreshold;
-	btScalar		m_angularSleepingThreshold;
+	btMatrix3x3 m_invInertiaTensorWorld;
+	btVector3 m_linearVelocity;
+	btVector3 m_angularVelocity;
+	btScalar m_inverseMass;
+	btVector3 m_linearFactor;
+
+	btVector3 m_gravity;
+	btVector3 m_gravity_acceleration;
+	btVector3 m_invInertiaLocal;
+	btVector3 m_totalForce;
+	btVector3 m_totalTorque;
+
+	btScalar m_linearDamping;
+	btScalar m_angularDamping;
+
+	bool m_additionalDamping;
+	btScalar m_additionalDampingFactor;
+	btScalar m_additionalLinearDampingThresholdSqr;
+	btScalar m_additionalAngularDampingThresholdSqr;
+	btScalar m_additionalAngularDampingFactor;
+
+	btScalar m_linearSleepingThreshold;
+	btScalar m_angularSleepingThreshold;
 
 	//m_optionalMotionState allows to automatic synchronize the world transform for active objects
-	btMotionState*	m_optionalMotionState;
+	btMotionState* m_optionalMotionState;
 
 	//keep track of typed constraints referencing this rigid body, to disable collision between linked bodies
 	btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
 
-	int				m_rigidbodyFlags;
-	
-	int				m_debugBodyId;
-	
-
-protected:
+	int m_rigidbodyFlags;
 
-	ATTRIBUTE_ALIGNED16(btVector3		m_deltaLinearVelocity);
-	btVector3		m_deltaAngularVelocity;
-	btVector3		m_angularFactor;
-	btVector3		m_invMass;
-	btVector3		m_pushVelocity;
-	btVector3		m_turnVelocity;
+	int m_debugBodyId;
 
+protected:
+	ATTRIBUTE_ALIGNED16(btVector3 m_deltaLinearVelocity);
+	btVector3 m_deltaAngularVelocity;
+	btVector3 m_angularFactor;
+	btVector3 m_invMass;
+	btVector3 m_pushVelocity;
+	btVector3 m_turnVelocity;
 
 public:
-
-
 	///The btRigidBodyConstructionInfo structure provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body.
 	///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument)
-	///You can use the motion state to synchronize the world transform between physics and graphics objects. 
+	///You can use the motion state to synchronize the world transform between physics and graphics objects.
 	///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state,
 	///m_startWorldTransform is only used when you don't provide a motion state.
-	struct	btRigidBodyConstructionInfo
+	struct btRigidBodyConstructionInfo
 	{
-		btScalar			m_mass;
+		btScalar m_mass;
 
 		///When a motionState is provided, the rigid body will initialize its world transform from the motion state
 		///In this case, m_startWorldTransform is ignored.
-		btMotionState*		m_motionState;
-		btTransform	m_startWorldTransform;
+		btMotionState* m_motionState;
+		btTransform m_startWorldTransform;
 
-		btCollisionShape*	m_collisionShape;
-		btVector3			m_localInertia;
-		btScalar			m_linearDamping;
-		btScalar			m_angularDamping;
+		btCollisionShape* m_collisionShape;
+		btVector3 m_localInertia;
+		btScalar m_linearDamping;
+		btScalar m_angularDamping;
 
 		///best simulation results when friction is non-zero
-		btScalar			m_friction;
+		btScalar m_friction;
 		///the m_rollingFriction prevents rounded shapes, such as spheres, cylinders and capsules from rolling forever.
 		///See Bullet/Demos/RollingFrictionDemo for usage
-		btScalar			m_rollingFriction;
+		btScalar m_rollingFriction;
+		btScalar m_spinningFriction;  //torsional friction around contact normal
+
 		///best simulation results using zero restitution.
-		btScalar			m_restitution;
+		btScalar m_restitution;
 
-		btScalar			m_linearSleepingThreshold;
-		btScalar			m_angularSleepingThreshold;
+		btScalar m_linearSleepingThreshold;
+		btScalar m_angularSleepingThreshold;
 
 		//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
 		//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
-		bool				m_additionalDamping;
-		btScalar			m_additionalDampingFactor;
-		btScalar			m_additionalLinearDampingThresholdSqr;
-		btScalar			m_additionalAngularDampingThresholdSqr;
-		btScalar			m_additionalAngularDampingFactor;
-
-		btRigidBodyConstructionInfo(	btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
-		m_mass(mass),
-			m_motionState(motionState),
-			m_collisionShape(collisionShape),
-			m_localInertia(localInertia),
-			m_linearDamping(btScalar(0.)),
-			m_angularDamping(btScalar(0.)),
-			m_friction(btScalar(0.5)),
-			m_rollingFriction(btScalar(0)),
-			m_restitution(btScalar(0.)),
-			m_linearSleepingThreshold(btScalar(0.8)),
-			m_angularSleepingThreshold(btScalar(1.f)),
-			m_additionalDamping(false),
-			m_additionalDampingFactor(btScalar(0.005)),
-			m_additionalLinearDampingThresholdSqr(btScalar(0.01)),
-			m_additionalAngularDampingThresholdSqr(btScalar(0.01)),
-			m_additionalAngularDampingFactor(btScalar(0.01))
+		bool m_additionalDamping;
+		btScalar m_additionalDampingFactor;
+		btScalar m_additionalLinearDampingThresholdSqr;
+		btScalar m_additionalAngularDampingThresholdSqr;
+		btScalar m_additionalAngularDampingFactor;
+
+		btRigidBodyConstructionInfo(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia = btVector3(0, 0, 0)) : m_mass(mass),
+																																									   m_motionState(motionState),
+																																									   m_collisionShape(collisionShape),
+																																									   m_localInertia(localInertia),
+																																									   m_linearDamping(btScalar(0.)),
+																																									   m_angularDamping(btScalar(0.)),
+																																									   m_friction(btScalar(0.5)),
+																																									   m_rollingFriction(btScalar(0)),
+																																									   m_spinningFriction(btScalar(0)),
+																																									   m_restitution(btScalar(0.)),
+																																									   m_linearSleepingThreshold(btScalar(0.8)),
+																																									   m_angularSleepingThreshold(btScalar(1.f)),
+																																									   m_additionalDamping(false),
+																																									   m_additionalDampingFactor(btScalar(0.005)),
+																																									   m_additionalLinearDampingThresholdSqr(btScalar(0.01)),
+																																									   m_additionalAngularDampingThresholdSqr(btScalar(0.01)),
+																																									   m_additionalAngularDampingFactor(btScalar(0.01))
 		{
 			m_startWorldTransform.setIdentity();
 		}
 	};
 
 	///btRigidBody constructor using construction info
-	btRigidBody(	const btRigidBodyConstructionInfo& constructionInfo);
+	btRigidBody(const btRigidBodyConstructionInfo& constructionInfo);
 
-	///btRigidBody constructor for backwards compatibility. 
+	///btRigidBody constructor for backwards compatibility.
 	///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo)
-	btRigidBody(	btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0));
-
+	btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia = btVector3(0, 0, 0));
 
 	virtual ~btRigidBody()
-        { 
-                //No constraints should point to this rigidbody
-		//Remove constraints from the dynamics world before you delete the related rigidbodies. 
-                btAssert(m_constraintRefs.size()==0); 
-        }
+	{
+		//No constraints should point to this rigidbody
+		//Remove constraints from the dynamics world before you delete the related rigidbodies.
+		btAssert(m_constraintRefs.size() == 0);
+	}
 
 protected:
-
 	///setupRigidBody is only used internally by the constructor
-	void	setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo);
+	void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo);
 
 public:
+	void proceedToTransform(const btTransform& newTrans);
 
-	void			proceedToTransform(const btTransform& newTrans); 
-	
 	///to keep collision detection and dynamics separate we don't store a rigidbody pointer
 	///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
-	static const btRigidBody*	upcast(const btCollisionObject* colObj)
+	static const btRigidBody* upcast(const btCollisionObject* colObj)
 	{
-		if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
+		if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
 			return (const btRigidBody*)colObj;
 		return 0;
 	}
-	static btRigidBody*	upcast(btCollisionObject* colObj)
+	static btRigidBody* upcast(btCollisionObject* colObj)
 	{
-		if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
+		if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)
 			return (btRigidBody*)colObj;
 		return 0;
 	}
-	
+
 	/// continuous collision detection needs prediction
-	void			predictIntegratedTransform(btScalar step, btTransform& predictedTransform) ;
-	
-	void			saveKinematicState(btScalar step);
-	
-	void			applyGravity();
-	
-	void			setGravity(const btVector3& acceleration);  
+	void predictIntegratedTransform(btScalar step, btTransform& predictedTransform);
+
+	void saveKinematicState(btScalar step);
+
+	void applyGravity();
+    
+    void clearGravity();
 
-	const btVector3&	getGravity() const
+	void setGravity(const btVector3& acceleration);
+
+	const btVector3& getGravity() const
 	{
 		return m_gravity_acceleration;
 	}
 
-	void			setDamping(btScalar lin_damping, btScalar ang_damping);
+	void setDamping(btScalar lin_damping, btScalar ang_damping);
 
 	btScalar getLinearDamping() const
 	{
@@ -246,18 +237,20 @@ public:
 		return m_angularSleepingThreshold;
 	}
 
-	void			applyDamping(btScalar timeStep);
+	void applyDamping(btScalar timeStep);
 
-	SIMD_FORCE_INLINE const btCollisionShape*	getCollisionShape() const {
+	SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const
+	{
 		return m_collisionShape;
 	}
 
-	SIMD_FORCE_INLINE btCollisionShape*	getCollisionShape() {
-			return m_collisionShape;
+	SIMD_FORCE_INLINE btCollisionShape* getCollisionShape()
+	{
+		return m_collisionShape;
 	}
-	
-	void			setMassProps(btScalar mass, const btVector3& inertia);
-	
+
+	void setMassProps(btScalar mass, const btVector3& inertia);
+
 	const btVector3& getLinearFactor() const
 	{
 		return m_linearFactor;
@@ -265,20 +258,22 @@ public:
 	void setLinearFactor(const btVector3& linearFactor)
 	{
 		m_linearFactor = linearFactor;
-		m_invMass = m_linearFactor*m_inverseMass;
+		m_invMass = m_linearFactor * m_inverseMass;
 	}
-	btScalar		getInvMass() const { return m_inverseMass; }
-	const btMatrix3x3& getInvInertiaTensorWorld() const { 
-		return m_invInertiaTensorWorld; 
+	btScalar getInvMass() const { return m_inverseMass; }
+	btScalar getMass() const { return m_inverseMass == btScalar(0.) ? btScalar(0.) : btScalar(1.0) / m_inverseMass; }
+	const btMatrix3x3& getInvInertiaTensorWorld() const
+	{
+		return m_invInertiaTensorWorld;
 	}
-		
-	void			integrateVelocities(btScalar step);
 
-	void			setCenterOfMassTransform(const btTransform& xform);
+	void integrateVelocities(btScalar step);
+
+	void setCenterOfMassTransform(const btTransform& xform);
 
-	void			applyCentralForce(const btVector3& force)
+	void applyCentralForce(const btVector3& force)
 	{
-		m_totalForce += force*m_linearFactor;
+		m_totalForce += force * m_linearFactor;
 	}
 
 	const btVector3& getTotalForce() const
@@ -290,90 +285,176 @@ public:
 	{
 		return m_totalTorque;
 	};
-    
+
 	const btVector3& getInvInertiaDiagLocal() const
 	{
 		return m_invInertiaLocal;
 	};
 
-	void	setInvInertiaDiagLocal(const btVector3& diagInvInertia)
+	void setInvInertiaDiagLocal(const btVector3& diagInvInertia)
 	{
 		m_invInertiaLocal = diagInvInertia;
 	}
 
-	void	setSleepingThresholds(btScalar linear,btScalar angular)
+	void setSleepingThresholds(btScalar linear, btScalar angular)
 	{
 		m_linearSleepingThreshold = linear;
 		m_angularSleepingThreshold = angular;
 	}
 
-	void	applyTorque(const btVector3& torque)
+	void applyTorque(const btVector3& torque)
 	{
-		m_totalTorque += torque*m_angularFactor;
+		m_totalTorque += torque * m_angularFactor;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_totalTorque);
+		#endif
 	}
-	
-	void	applyForce(const btVector3& force, const btVector3& rel_pos) 
+
+	void applyForce(const btVector3& force, const btVector3& rel_pos)
 	{
 		applyCentralForce(force);
-		applyTorque(rel_pos.cross(force*m_linearFactor));
+		applyTorque(rel_pos.cross(force * m_linearFactor));
 	}
-	
+
 	void applyCentralImpulse(const btVector3& impulse)
 	{
-		m_linearVelocity += impulse *m_linearFactor * m_inverseMass;
+		m_linearVelocity += impulse * m_linearFactor * m_inverseMass;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_linearVelocity);
+		#endif
 	}
-	
-  	void applyTorqueImpulse(const btVector3& torque)
+
+	void applyTorqueImpulse(const btVector3& torque)
 	{
-			m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+		m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_angularVelocity);
+		#endif
 	}
-	
-	void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) 
+
+	void applyImpulse(const btVector3& impulse, const btVector3& rel_pos)
 	{
 		if (m_inverseMass != btScalar(0.))
 		{
 			applyCentralImpulse(impulse);
 			if (m_angularFactor)
 			{
-				applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor));
+				applyTorqueImpulse(rel_pos.cross(impulse * m_linearFactor));
 			}
 		}
 	}
+    
+    void applyPushImpulse(const btVector3& impulse, const btVector3& rel_pos)
+    {
+        if (m_inverseMass != btScalar(0.))
+        {
+            applyCentralPushImpulse(impulse);
+            if (m_angularFactor)
+            {
+                applyTorqueTurnImpulse(rel_pos.cross(impulse * m_linearFactor));
+            }
+        }
+    }
+    
+    btVector3 getPushVelocity() const
+    {
+        return m_pushVelocity;
+    }
+    
+    btVector3 getTurnVelocity() const
+    {
+        return m_turnVelocity;
+    }
+    
+    void setPushVelocity(const btVector3& v)
+    {
+        m_pushVelocity = v;
+    }
+
+    #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+    void clampVelocity(btVector3& v) const {
+        v.setX(
+            fmax(-BT_CLAMP_VELOCITY_TO,
+                 fmin(BT_CLAMP_VELOCITY_TO, v.getX()))
+        );
+        v.setY(
+            fmax(-BT_CLAMP_VELOCITY_TO,
+                 fmin(BT_CLAMP_VELOCITY_TO, v.getY()))
+        );
+        v.setZ(
+            fmax(-BT_CLAMP_VELOCITY_TO,
+                 fmin(BT_CLAMP_VELOCITY_TO, v.getZ()))
+        );
+    }
+    #endif
+
+    void setTurnVelocity(const btVector3& v)
+    {
+        m_turnVelocity = v;
+        #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+        clampVelocity(m_turnVelocity);
+        #endif
+    }
+    
+    void applyCentralPushImpulse(const btVector3& impulse)
+    {
+        m_pushVelocity += impulse * m_linearFactor * m_inverseMass;
+        #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+        clampVelocity(m_pushVelocity);
+        #endif
+    }
+    
+    void applyTorqueTurnImpulse(const btVector3& torque)
+    {
+        m_turnVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+        #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+        clampVelocity(m_turnVelocity);
+        #endif
+    }
 
-	void clearForces() 
+	void clearForces()
 	{
 		m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 		m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
 	}
-	
-	void updateInertiaTensor();    
-	
-	const btVector3&     getCenterOfMassPosition() const { 
-		return m_worldTransform.getOrigin(); 
+
+	void updateInertiaTensor();
+
+	const btVector3& getCenterOfMassPosition() const
+	{
+		return m_worldTransform.getOrigin();
 	}
 	btQuaternion getOrientation() const;
-	
-	const btTransform&  getCenterOfMassTransform() const { 
-		return m_worldTransform; 
+
+	const btTransform& getCenterOfMassTransform() const
+	{
+		return m_worldTransform;
 	}
-	const btVector3&   getLinearVelocity() const { 
-		return m_linearVelocity; 
+	const btVector3& getLinearVelocity() const
+	{
+		return m_linearVelocity;
 	}
-	const btVector3&    getAngularVelocity() const { 
-		return m_angularVelocity; 
+	const btVector3& getAngularVelocity() const
+	{
+		return m_angularVelocity;
 	}
-	
 
 	inline void setLinearVelocity(const btVector3& lin_vel)
-	{ 
+	{
 		m_updateRevision++;
-		m_linearVelocity = lin_vel; 
+		m_linearVelocity = lin_vel;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_linearVelocity);
+		#endif
 	}
 
-	inline void setAngularVelocity(const btVector3& ang_vel) 
-	{ 
+	inline void setAngularVelocity(const btVector3& ang_vel)
+	{
 		m_updateRevision++;
-		m_angularVelocity = ang_vel; 
+		m_angularVelocity = ang_vel;
+		#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+		clampVelocity(m_angularVelocity);
+		#endif
 	}
 
 	btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const
@@ -384,19 +465,20 @@ public:
 		//for kinematic objects, we could also use use:
 		//		return 	(m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
 	}
+    
+    btVector3 getPushVelocityInLocalPoint(const btVector3& rel_pos) const
+    {
+        //we also calculate lin/ang velocity for kinematic objects
+        return m_pushVelocity + m_turnVelocity.cross(rel_pos);
+    }
 
-	void translate(const btVector3& v) 
+	void translate(const btVector3& v)
 	{
-		m_worldTransform.getOrigin() += v; 
+		m_worldTransform.getOrigin() += v;
 	}
 
-	
-	void	getAabb(btVector3& aabbMin,btVector3& aabbMax) const;
-
-
+	void getAabb(btVector3& aabbMin, btVector3& aabbMax) const;
 
-
-	
 	SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const
 	{
 		btVector3 r0 = pos - getCenterOfMassPosition();
@@ -406,7 +488,6 @@ public:
 		btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0);
 
 		return m_inverseMass + normal.dot(vec);
-
 	}
 
 	SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const
@@ -415,26 +496,25 @@ public:
 		return axis.dot(vec);
 	}
 
-	SIMD_FORCE_INLINE void	updateDeactivation(btScalar timeStep)
+	SIMD_FORCE_INLINE void updateDeactivation(btScalar timeStep)
 	{
-		if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
+		if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
 			return;
 
-		if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) &&
-			(getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold))
+		if ((getLinearVelocity().length2() < m_linearSleepingThreshold * m_linearSleepingThreshold) &&
+			(getAngularVelocity().length2() < m_angularSleepingThreshold * m_angularSleepingThreshold))
 		{
 			m_deactivationTime += timeStep;
-		} else
+		}
+		else
 		{
-			m_deactivationTime=btScalar(0.);
+			m_deactivationTime = btScalar(0.);
 			setActivationState(0);
 		}
-
 	}
 
-	SIMD_FORCE_INLINE bool	wantsSleeping()
+	SIMD_FORCE_INLINE bool wantsSleeping()
 	{
-
 		if (getActivationState() == DISABLE_DEACTIVATION)
 			return false;
 
@@ -442,41 +522,39 @@ public:
 		if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
 			return false;
 
-		if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
+		if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
 			return true;
 
-		if (m_deactivationTime> gDeactivationTime)
+		if (m_deactivationTime > gDeactivationTime)
 		{
 			return true;
 		}
 		return false;
 	}
 
-
-	
-	const btBroadphaseProxy*	getBroadphaseProxy() const
+	const btBroadphaseProxy* getBroadphaseProxy() const
 	{
 		return m_broadphaseHandle;
 	}
-	btBroadphaseProxy*	getBroadphaseProxy() 
+	btBroadphaseProxy* getBroadphaseProxy()
 	{
 		return m_broadphaseHandle;
 	}
-	void	setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy)
+	void setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy)
 	{
 		m_broadphaseHandle = broadphaseProxy;
 	}
 
 	//btMotionState allows to automatic synchronize the world transform for active objects
-	btMotionState*	getMotionState()
+	btMotionState* getMotionState()
 	{
 		return m_optionalMotionState;
 	}
-	const btMotionState*	getMotionState() const
+	const btMotionState* getMotionState() const
 	{
 		return m_optionalMotionState;
 	}
-	void	setMotionState(btMotionState* motionState)
+	void setMotionState(btMotionState* motionState)
 	{
 		m_optionalMotionState = motionState;
 		if (m_optionalMotionState)
@@ -484,33 +562,31 @@ public:
 	}
 
 	//for experimental overriding of friction/contact solver func
-	int	m_contactSolverType;
-	int	m_frictionSolverType;
+	int m_contactSolverType;
+	int m_frictionSolverType;
 
-	void	setAngularFactor(const btVector3& angFac)
+	void setAngularFactor(const btVector3& angFac)
 	{
 		m_updateRevision++;
 		m_angularFactor = angFac;
 	}
 
-	void	setAngularFactor(btScalar angFac)
+	void setAngularFactor(btScalar angFac)
 	{
 		m_updateRevision++;
-		m_angularFactor.setValue(angFac,angFac,angFac);
+		m_angularFactor.setValue(angFac, angFac, angFac);
 	}
-	const btVector3&	getAngularFactor() const
+	const btVector3& getAngularFactor() const
 	{
 		return m_angularFactor;
 	}
 
 	//is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase?
-	bool	isInWorld() const
+	bool isInWorld() const
 	{
 		return (getBroadphaseProxy() != 0);
 	}
 
-	virtual bool checkCollideWithOverride(const  btCollisionObject* co) const;
-
 	void addConstraintRef(btTypedConstraint* c);
 	void removeConstraintRef(btTypedConstraint* c);
 
@@ -524,7 +600,7 @@ public:
 		return m_constraintRefs.size();
 	}
 
-	void	setFlags(int flags)
+	void setFlags(int flags)
 	{
 		m_rigidbodyFlags = flags;
 	}
@@ -534,12 +610,9 @@ public:
 		return m_rigidbodyFlags;
 	}
 
-
-	
-
 	///perform implicit force computation in world space
 	btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const;
-	
+
 	///perform implicit force computation in body space (inertial frame)
 	btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const;
 
@@ -549,70 +622,66 @@ public:
 
 	///////////////////////////////////////////////
 
-	virtual	int	calculateSerializeBufferSize()	const;
+	virtual int calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer,  class btSerializer* serializer) const;
+	virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
 
 	virtual void serializeSingleObject(class btSerializer* serializer) const;
-
 };
 
 //@todo add m_optionalMotionState and m_constraintRefs to btRigidBodyData
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btRigidBodyFloatData
+struct btRigidBodyFloatData
 {
-	btCollisionObjectFloatData	m_collisionObjectData;
-	btMatrix3x3FloatData		m_invInertiaTensorWorld;
-	btVector3FloatData		m_linearVelocity;
-	btVector3FloatData		m_angularVelocity;
-	btVector3FloatData		m_angularFactor;
-	btVector3FloatData		m_linearFactor;
-	btVector3FloatData		m_gravity;	
-	btVector3FloatData		m_gravity_acceleration;
-	btVector3FloatData		m_invInertiaLocal;
-	btVector3FloatData		m_totalForce;
-	btVector3FloatData		m_totalTorque;
-	float					m_inverseMass;
-	float					m_linearDamping;
-	float					m_angularDamping;
-	float					m_additionalDampingFactor;
-	float					m_additionalLinearDampingThresholdSqr;
-	float					m_additionalAngularDampingThresholdSqr;
-	float					m_additionalAngularDampingFactor;
-	float					m_linearSleepingThreshold;
-	float					m_angularSleepingThreshold;
-	int						m_additionalDamping;
+	btCollisionObjectFloatData m_collisionObjectData;
+	btMatrix3x3FloatData m_invInertiaTensorWorld;
+	btVector3FloatData m_linearVelocity;
+	btVector3FloatData m_angularVelocity;
+	btVector3FloatData m_angularFactor;
+	btVector3FloatData m_linearFactor;
+	btVector3FloatData m_gravity;
+	btVector3FloatData m_gravity_acceleration;
+	btVector3FloatData m_invInertiaLocal;
+	btVector3FloatData m_totalForce;
+	btVector3FloatData m_totalTorque;
+	float m_inverseMass;
+	float m_linearDamping;
+	float m_angularDamping;
+	float m_additionalDampingFactor;
+	float m_additionalLinearDampingThresholdSqr;
+	float m_additionalAngularDampingThresholdSqr;
+	float m_additionalAngularDampingFactor;
+	float m_linearSleepingThreshold;
+	float m_angularSleepingThreshold;
+	int m_additionalDamping;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btRigidBodyDoubleData
+struct btRigidBodyDoubleData
 {
-	btCollisionObjectDoubleData	m_collisionObjectData;
-	btMatrix3x3DoubleData		m_invInertiaTensorWorld;
-	btVector3DoubleData		m_linearVelocity;
-	btVector3DoubleData		m_angularVelocity;
-	btVector3DoubleData		m_angularFactor;
-	btVector3DoubleData		m_linearFactor;
-	btVector3DoubleData		m_gravity;	
-	btVector3DoubleData		m_gravity_acceleration;
-	btVector3DoubleData		m_invInertiaLocal;
-	btVector3DoubleData		m_totalForce;
-	btVector3DoubleData		m_totalTorque;
-	double					m_inverseMass;
-	double					m_linearDamping;
-	double					m_angularDamping;
-	double					m_additionalDampingFactor;
-	double					m_additionalLinearDampingThresholdSqr;
-	double					m_additionalAngularDampingThresholdSqr;
-	double					m_additionalAngularDampingFactor;
-	double					m_linearSleepingThreshold;
-	double					m_angularSleepingThreshold;
-	int						m_additionalDamping;
-	char	m_padding[4];
+	btCollisionObjectDoubleData m_collisionObjectData;
+	btMatrix3x3DoubleData m_invInertiaTensorWorld;
+	btVector3DoubleData m_linearVelocity;
+	btVector3DoubleData m_angularVelocity;
+	btVector3DoubleData m_angularFactor;
+	btVector3DoubleData m_linearFactor;
+	btVector3DoubleData m_gravity;
+	btVector3DoubleData m_gravity_acceleration;
+	btVector3DoubleData m_invInertiaLocal;
+	btVector3DoubleData m_totalForce;
+	btVector3DoubleData m_totalTorque;
+	double m_inverseMass;
+	double m_linearDamping;
+	double m_angularDamping;
+	double m_additionalDampingFactor;
+	double m_additionalLinearDampingThresholdSqr;
+	double m_additionalAngularDampingThresholdSqr;
+	double m_additionalAngularDampingFactor;
+	double m_linearSleepingThreshold;
+	double m_angularSleepingThreshold;
+	int m_additionalDamping;
+	char m_padding[4];
 };
 
-
-
-#endif //BT_RIGIDBODY_H
-
+#endif  //BT_RIGIDBODY_H
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
index 35dd38840f6..8103390fb1d 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
@@ -21,47 +21,40 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
 
-
 /*
   Make sure this dummy function never changes so that it
   can be used by probes that are checking whether the
   library is actually installed.
 */
-extern "C" 
+extern "C"
 {
-	void btBulletDynamicsProbe ();
-	void btBulletDynamicsProbe () {}
+	void btBulletDynamicsProbe();
+	void btBulletDynamicsProbe() {}
 }
 
-
-
-
-btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
-:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
-m_constraintSolver(constraintSolver),
-m_ownsConstraintSolver(false),
-m_gravity(0,0,-10)
+btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
+	: btDynamicsWorld(dispatcher, pairCache, collisionConfiguration),
+	  m_constraintSolver(constraintSolver),
+	  m_ownsConstraintSolver(false),
+	  m_gravity(0, 0, -10)
 {
-
 }
 
-
 btSimpleDynamicsWorld::~btSimpleDynamicsWorld()
 {
 	if (m_ownsConstraintSolver)
-		btAlignedFree( m_constraintSolver);
+		btAlignedFree(m_constraintSolver);
 }
 
-int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
+int btSimpleDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
 {
 	(void)fixedTimeStep;
 	(void)maxSubSteps;
 
-
 	///apply gravity, predict motion
 	predictUnconstraintMotion(timeStep);
 
-	btDispatcherInfo&	dispatchInfo = getDispatchInfo();
+	btDispatcherInfo& dispatchInfo = getDispatchInfo();
 	dispatchInfo.m_timeStep = timeStep;
 	dispatchInfo.m_stepCount = 0;
 	dispatchInfo.m_debugDraw = getDebugDrawer();
@@ -74,17 +67,17 @@ int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b
 	if (numManifolds)
 	{
 		btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer();
-		
+
 		btContactSolverInfo infoGlobal;
 		infoGlobal.m_timeStep = timeStep;
-		m_constraintSolver->prepareSolve(0,numManifolds);
-		m_constraintSolver->solveGroup(&getCollisionObjectArray()[0],getNumCollisionObjects(),manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_dispatcher1);
-		m_constraintSolver->allSolved(infoGlobal,m_debugDrawer);
+		m_constraintSolver->prepareSolve(0, numManifolds);
+		m_constraintSolver->solveGroup(&getCollisionObjectArray()[0], getNumCollisionObjects(), manifoldPtr, numManifolds, 0, 0, infoGlobal, m_debugDrawer, m_dispatcher1);
+		m_constraintSolver->allSolved(infoGlobal, m_debugDrawer);
 	}
 
 	///integrate transforms
 	integrateTransforms(timeStep);
-		
+
 	updateAabbs();
 
 	synchronizeMotionStates();
@@ -92,29 +85,27 @@ int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b
 	clearForces();
 
 	return 1;
-
 }
 
-void	btSimpleDynamicsWorld::clearForces()
+void btSimpleDynamicsWorld::clearForces()
 {
 	///@todo: iterate over awake simulation islands!
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for (int i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
-		
+
 		btRigidBody* body = btRigidBody::upcast(colObj);
 		if (body)
 		{
 			body->clearForces();
 		}
 	}
-}	
-
+}
 
-void	btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
+void btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
 {
 	m_gravity = gravity;
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for (int i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -125,17 +116,17 @@ void	btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
 	}
 }
 
-btVector3 btSimpleDynamicsWorld::getGravity () const
+btVector3 btSimpleDynamicsWorld::getGravity() const
 {
 	return m_gravity;
 }
 
-void	btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
+void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
 {
 	btCollisionWorld::removeCollisionObject(body);
 }
 
-void	btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
 {
 	btRigidBody* body = btRigidBody::upcast(collisionObject);
 	if (body)
@@ -144,8 +135,7 @@ void	btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionOb
 		btCollisionWorld::removeCollisionObject(collisionObject);
 }
 
-
-void	btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
+void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
 {
 	body->setGravity(m_gravity);
 
@@ -155,37 +145,32 @@ void	btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
 	}
 }
 
-void	btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
+void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask)
 {
 	body->setGravity(m_gravity);
 
 	if (body->getCollisionShape())
 	{
-		addCollisionObject(body,group,mask);
+		addCollisionObject(body, group, mask);
 	}
 }
 
-
-void	btSimpleDynamicsWorld::debugDrawWorld()
+void btSimpleDynamicsWorld::debugDrawWorld()
 {
-
 }
-				
-void	btSimpleDynamicsWorld::addAction(btActionInterface* action)
-{
 
+void btSimpleDynamicsWorld::addAction(btActionInterface* action)
+{
 }
 
-void	btSimpleDynamicsWorld::removeAction(btActionInterface* action)
+void btSimpleDynamicsWorld::removeAction(btActionInterface* action)
 {
-
 }
 
-
-void	btSimpleDynamicsWorld::updateAabbs()
+void btSimpleDynamicsWorld::updateAabbs()
 {
 	btTransform predictedTrans;
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for (int i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -193,19 +178,19 @@ void	btSimpleDynamicsWorld::updateAabbs()
 		{
 			if (body->isActive() && (!body->isStaticObject()))
 			{
-				btVector3 minAabb,maxAabb;
-				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+				btVector3 minAabb, maxAabb;
+				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb, maxAabb);
 				btBroadphaseInterface* bp = getBroadphase();
-				bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
+				bp->setAabb(body->getBroadphaseHandle(), minAabb, maxAabb, m_dispatcher1);
 			}
 		}
 	}
 }
 
-void	btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
+void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
 	btTransform predictedTrans;
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for (int i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -214,17 +199,15 @@ void	btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
 			if (body->isActive() && (!body->isStaticObject()))
 			{
 				body->predictIntegratedTransform(timeStep, predictedTrans);
-				body->proceedToTransform( predictedTrans);
+				body->proceedToTransform(predictedTrans);
 			}
 		}
 	}
 }
 
-
-
-void	btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 {
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for (int i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -235,20 +218,19 @@ void	btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 				if (body->isActive())
 				{
 					body->applyGravity();
-					body->integrateVelocities( timeStep);
+					body->integrateVelocities(timeStep);
 					body->applyDamping(timeStep);
-					body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
+					body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform());
 				}
 			}
 		}
 	}
 }
 
-
-void	btSimpleDynamicsWorld::synchronizeMotionStates()
+void btSimpleDynamicsWorld::synchronizeMotionStates()
 {
 	///@todo: iterate over awake simulation islands!
-	for ( int i=0;i<m_collisionObjects.size();i++)
+	for (int i = 0; i < m_collisionObjects.size(); i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -260,11 +242,9 @@ void	btSimpleDynamicsWorld::synchronizeMotionStates()
 			}
 		}
 	}
-
 }
 
-
-void	btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
+void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 {
 	if (m_ownsConstraintSolver)
 	{
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
index d48d2e39c4d..12be231c7f1 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
@@ -27,63 +27,58 @@ class btConstraintSolver;
 class btSimpleDynamicsWorld : public btDynamicsWorld
 {
 protected:
+	btConstraintSolver* m_constraintSolver;
 
-	btConstraintSolver*	m_constraintSolver;
+	bool m_ownsConstraintSolver;
 
-	bool	m_ownsConstraintSolver;
-
-	void	predictUnconstraintMotion(btScalar timeStep);
-	
-	void	integrateTransforms(btScalar timeStep);
-		
-	btVector3	m_gravity;
-	
-public:
+	void predictUnconstraintMotion(btScalar timeStep);
 
+	void integrateTransforms(btScalar timeStep);
 
+	btVector3 m_gravity;
 
+public:
 	///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver
-	btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+	btSimpleDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration);
 
 	virtual ~btSimpleDynamicsWorld();
-		
+
 	///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead
-	virtual int	stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
+	virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
 
-	virtual void	setGravity(const btVector3& gravity);
+	virtual void setGravity(const btVector3& gravity);
 
-	virtual btVector3 getGravity () const;
+	virtual btVector3 getGravity() const;
 
-	virtual void	addRigidBody(btRigidBody* body);
+	virtual void addRigidBody(btRigidBody* body);
 
-	virtual void	addRigidBody(btRigidBody* body, short group, short mask);
+	virtual void addRigidBody(btRigidBody* body, int group, int mask);
 
-	virtual void	removeRigidBody(btRigidBody* body);
+	virtual void removeRigidBody(btRigidBody* body);
 
-	virtual void	debugDrawWorld();
-				
-	virtual void	addAction(btActionInterface* action);
+	virtual void debugDrawWorld();
 
-	virtual void	removeAction(btActionInterface* action);
+	virtual void addAction(btActionInterface* action);
+
+	virtual void removeAction(btActionInterface* action);
 
 	///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
-	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
-	
-	virtual void	updateAabbs();
+	virtual void removeCollisionObject(btCollisionObject* collisionObject);
+
+	virtual void updateAabbs();
 
-	virtual void	synchronizeMotionStates();
+	virtual void synchronizeMotionStates();
 
-	virtual void	setConstraintSolver(btConstraintSolver* solver);
+	virtual void setConstraintSolver(btConstraintSolver* solver);
 
 	virtual btConstraintSolver* getConstraintSolver();
 
-	virtual btDynamicsWorldType	getWorldType() const
+	virtual btDynamicsWorldType getWorldType() const
 	{
 		return BT_SIMPLE_DYNAMICS_WORLD;
 	}
 
-	virtual void	clearForces();
-
+	virtual void clearForces();
 };
 
-#endif //BT_SIMPLE_DYNAMICS_WORLD_H
+#endif  //BT_SIMPLE_DYNAMICS_WORLD_H
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp
new file mode 100644
index 00000000000..772b7742023
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp
@@ -0,0 +1,696 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btThreads.h"
+#include "btSimulationIslandManagerMt.h"
+#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h"  // for s_minimumContactManifoldsForBatching
+
+//#include <stdio.h>
+#include "LinearMath/btQuickprof.h"
+
+SIMD_FORCE_INLINE int calcBatchCost(int bodies, int manifolds, int constraints)
+{
+	// rough estimate of the cost of a batch, used for merging
+	int batchCost = bodies + 8 * manifolds + 4 * constraints;
+	return batchCost;
+}
+
+SIMD_FORCE_INLINE int calcBatchCost(const btSimulationIslandManagerMt::Island* island)
+{
+	return calcBatchCost(island->bodyArray.size(), island->manifoldArray.size(), island->constraintArray.size());
+}
+
+btSimulationIslandManagerMt::btSimulationIslandManagerMt()
+{
+	m_minimumSolverBatchSize = calcBatchCost(0, 128, 0);
+	m_batchIslandMinBodyCount = 32;
+	m_islandDispatch = parallelIslandDispatch;
+	m_batchIsland = NULL;
+}
+
+btSimulationIslandManagerMt::~btSimulationIslandManagerMt()
+{
+	for (int i = 0; i < m_allocatedIslands.size(); ++i)
+	{
+		delete m_allocatedIslands[i];
+	}
+	m_allocatedIslands.resize(0);
+	m_activeIslands.resize(0);
+	m_freeIslands.resize(0);
+}
+
+inline int getIslandId(const btPersistentManifold* lhs)
+{
+	const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
+	const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
+	int islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag();
+	return islandId;
+}
+
+SIMD_FORCE_INLINE int btGetConstraintIslandId1(const btTypedConstraint* lhs)
+{
+	const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
+	const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
+	int islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
+	return islandId;
+}
+
+/// function object that routes calls to operator<
+class IslandBatchSizeSortPredicate
+{
+public:
+	bool operator()(const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs) const
+	{
+		int lCost = calcBatchCost(lhs);
+		int rCost = calcBatchCost(rhs);
+		return lCost > rCost;
+	}
+};
+
+class IslandBodyCapacitySortPredicate
+{
+public:
+	bool operator()(const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs) const
+	{
+		return lhs->bodyArray.capacity() > rhs->bodyArray.capacity();
+	}
+};
+
+void btSimulationIslandManagerMt::Island::append(const Island& other)
+{
+	// append bodies
+	for (int i = 0; i < other.bodyArray.size(); ++i)
+	{
+		bodyArray.push_back(other.bodyArray[i]);
+	}
+	// append manifolds
+	for (int i = 0; i < other.manifoldArray.size(); ++i)
+	{
+		manifoldArray.push_back(other.manifoldArray[i]);
+	}
+	// append constraints
+	for (int i = 0; i < other.constraintArray.size(); ++i)
+	{
+		constraintArray.push_back(other.constraintArray[i]);
+	}
+}
+
+bool btIsBodyInIsland(const btSimulationIslandManagerMt::Island& island, const btCollisionObject* obj)
+{
+	for (int i = 0; i < island.bodyArray.size(); ++i)
+	{
+		if (island.bodyArray[i] == obj)
+		{
+			return true;
+		}
+	}
+	return false;
+}
+
+void btSimulationIslandManagerMt::initIslandPools()
+{
+	// reset island pools
+	int numElem = getUnionFind().getNumElements();
+	m_lookupIslandFromId.resize(numElem);
+	for (int i = 0; i < m_lookupIslandFromId.size(); ++i)
+	{
+		m_lookupIslandFromId[i] = NULL;
+	}
+	m_activeIslands.resize(0);
+	m_freeIslands.resize(0);
+	// check whether allocated islands are sorted by body capacity (largest to smallest)
+	int lastCapacity = 0;
+	bool isSorted = true;
+	for (int i = 0; i < m_allocatedIslands.size(); ++i)
+	{
+		Island* island = m_allocatedIslands[i];
+		int cap = island->bodyArray.capacity();
+		if (cap > lastCapacity)
+		{
+			isSorted = false;
+			break;
+		}
+		lastCapacity = cap;
+	}
+	if (!isSorted)
+	{
+		m_allocatedIslands.quickSort(IslandBodyCapacitySortPredicate());
+	}
+
+	m_batchIsland = NULL;
+	// mark all islands free (but avoid deallocation)
+	for (int i = 0; i < m_allocatedIslands.size(); ++i)
+	{
+		Island* island = m_allocatedIslands[i];
+		island->bodyArray.resize(0);
+		island->manifoldArray.resize(0);
+		island->constraintArray.resize(0);
+		island->id = -1;
+		island->isSleeping = true;
+		m_freeIslands.push_back(island);
+	}
+}
+
+btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland(int id)
+{
+	btAssert(id >= 0);
+	btAssert(id < m_lookupIslandFromId.size());
+	Island* island = m_lookupIslandFromId[id];
+	if (island == NULL)
+	{
+		// search for existing island
+		for (int i = 0; i < m_activeIslands.size(); ++i)
+		{
+			if (m_activeIslands[i]->id == id)
+			{
+				island = m_activeIslands[i];
+				break;
+			}
+		}
+		m_lookupIslandFromId[id] = island;
+	}
+	return island;
+}
+
+btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::allocateIsland(int id, int numBodies)
+{
+	Island* island = NULL;
+	int allocSize = numBodies;
+	if (numBodies < m_batchIslandMinBodyCount)
+	{
+		if (m_batchIsland)
+		{
+			island = m_batchIsland;
+			m_lookupIslandFromId[id] = island;
+			// if we've made a large enough batch,
+			if (island->bodyArray.size() + numBodies >= m_batchIslandMinBodyCount)
+			{
+				// next time start a new batch
+				m_batchIsland = NULL;
+			}
+			return island;
+		}
+		else
+		{
+			// need to allocate a batch island
+			allocSize = m_batchIslandMinBodyCount * 2;
+		}
+	}
+	btAlignedObjectArray<Island*>& freeIslands = m_freeIslands;
+
+	// search for free island
+	if (freeIslands.size() > 0)
+	{
+		// try to reuse a previously allocated island
+		int iFound = freeIslands.size();
+		// linear search for smallest island that can hold our bodies
+		for (int i = freeIslands.size() - 1; i >= 0; --i)
+		{
+			if (freeIslands[i]->bodyArray.capacity() >= allocSize)
+			{
+				iFound = i;
+				island = freeIslands[i];
+				island->id = id;
+				break;
+			}
+		}
+		// if found, shrink array while maintaining ordering
+		if (island)
+		{
+			int iDest = iFound;
+			int iSrc = iDest + 1;
+			while (iSrc < freeIslands.size())
+			{
+				freeIslands[iDest++] = freeIslands[iSrc++];
+			}
+			freeIslands.pop_back();
+		}
+	}
+	if (island == NULL)
+	{
+		// no free island found, allocate
+		island = new Island();  // TODO: change this to use the pool allocator
+		island->id = id;
+		island->bodyArray.reserve(allocSize);
+		m_allocatedIslands.push_back(island);
+	}
+	m_lookupIslandFromId[id] = island;
+	if (numBodies < m_batchIslandMinBodyCount)
+	{
+		m_batchIsland = island;
+	}
+	m_activeIslands.push_back(island);
+	return island;
+}
+
+void btSimulationIslandManagerMt::buildIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld)
+{
+	BT_PROFILE("buildIslands");
+
+	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+
+	//we are going to sort the unionfind array, and store the element id in the size
+	//afterwards, we clean unionfind, to make sure no-one uses it anymore
+
+	getUnionFind().sortIslands();
+	int numElem = getUnionFind().getNumElements();
+
+	int endIslandIndex = 1;
+	int startIslandIndex;
+
+	//update the sleeping state for bodies, if all are sleeping
+	for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
+	{
+		int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+		for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++)
+		{
+		}
+
+		//int numSleeping = 0;
+
+		bool allSleeping = true;
+
+		int idx;
+		for (idx = startIslandIndex; idx < endIslandIndex; idx++)
+		{
+			int i = getUnionFind().getElement(idx).m_sz;
+
+			btCollisionObject* colObj0 = collisionObjects[i];
+			if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+			{
+				//				printf("error in island management\n");
+			}
+
+			btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+			if (colObj0->getIslandTag() == islandId)
+			{
+				if (colObj0->getActivationState() == ACTIVE_TAG ||
+					colObj0->getActivationState() == DISABLE_DEACTIVATION)
+				{
+					allSleeping = false;
+					break;
+				}
+			}
+		}
+
+		if (allSleeping)
+		{
+			int idx;
+			for (idx = startIslandIndex; idx < endIslandIndex; idx++)
+			{
+				int i = getUnionFind().getElement(idx).m_sz;
+				btCollisionObject* colObj0 = collisionObjects[i];
+				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+				{
+					//					printf("error in island management\n");
+				}
+
+				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+				if (colObj0->getIslandTag() == islandId)
+				{
+					colObj0->setActivationState(ISLAND_SLEEPING);
+				}
+			}
+		}
+		else
+		{
+			int idx;
+			for (idx = startIslandIndex; idx < endIslandIndex; idx++)
+			{
+				int i = getUnionFind().getElement(idx).m_sz;
+
+				btCollisionObject* colObj0 = collisionObjects[i];
+				if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
+				{
+					//					printf("error in island management\n");
+				}
+
+				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+				if (colObj0->getIslandTag() == islandId)
+				{
+					if (colObj0->getActivationState() == ISLAND_SLEEPING)
+					{
+						colObj0->setActivationState(WANTS_DEACTIVATION);
+						colObj0->setDeactivationTime(0.f);
+					}
+				}
+			}
+		}
+	}
+}
+
+void btSimulationIslandManagerMt::addBodiesToIslands(btCollisionWorld* collisionWorld)
+{
+	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+	int endIslandIndex = 1;
+	int startIslandIndex;
+	int numElem = getUnionFind().getNumElements();
+
+	// create explicit islands and add bodies to each
+	for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
+	{
+		int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+
+		// find end index
+		for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++)
+		{
+		}
+		// check if island is sleeping
+		bool islandSleeping = true;
+		for (int iElem = startIslandIndex; iElem < endIslandIndex; iElem++)
+		{
+			int i = getUnionFind().getElement(iElem).m_sz;
+			btCollisionObject* colObj = collisionObjects[i];
+			if (colObj->isActive())
+			{
+				islandSleeping = false;
+			}
+		}
+		if (!islandSleeping)
+		{
+			// want to count the number of bodies before allocating the island to optimize memory usage of the Island structures
+			int numBodies = endIslandIndex - startIslandIndex;
+			Island* island = allocateIsland(islandId, numBodies);
+			island->isSleeping = false;
+
+			// add bodies to island
+			for (int iElem = startIslandIndex; iElem < endIslandIndex; iElem++)
+			{
+				int i = getUnionFind().getElement(iElem).m_sz;
+				btCollisionObject* colObj = collisionObjects[i];
+				island->bodyArray.push_back(colObj);
+			}
+		}
+	}
+}
+
+void btSimulationIslandManagerMt::addManifoldsToIslands(btDispatcher* dispatcher)
+{
+	// walk all the manifolds, activating bodies touched by kinematic objects, and add each manifold to its Island
+	int maxNumManifolds = dispatcher->getNumManifolds();
+	for (int i = 0; i < maxNumManifolds; i++)
+	{
+		btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
+
+		const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
+		const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
+
+		///@todo: check sleeping conditions!
+		if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
+			((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
+		{
+			//kinematic objects don't merge islands, but wake up all connected objects
+			if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
+			{
+				if (colObj0->hasContactResponse())
+					colObj1->activate();
+			}
+			if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
+			{
+				if (colObj1->hasContactResponse())
+					colObj0->activate();
+			}
+			//filtering for response
+			if (dispatcher->needsResponse(colObj0, colObj1))
+			{
+				// scatter manifolds into various islands
+				int islandId = getIslandId(manifold);
+				// if island not sleeping,
+				if (Island* island = getIsland(islandId))
+				{
+					island->manifoldArray.push_back(manifold);
+				}
+			}
+		}
+	}
+}
+
+void btSimulationIslandManagerMt::addConstraintsToIslands(btAlignedObjectArray<btTypedConstraint*>& constraints)
+{
+	// walk constraints
+	for (int i = 0; i < constraints.size(); i++)
+	{
+		// scatter constraints into various islands
+		btTypedConstraint* constraint = constraints[i];
+		if (constraint->isEnabled())
+		{
+			int islandId = btGetConstraintIslandId1(constraint);
+			// if island is not sleeping,
+			if (Island* island = getIsland(islandId))
+			{
+				island->constraintArray.push_back(constraint);
+			}
+		}
+	}
+}
+
+void btSimulationIslandManagerMt::mergeIslands()
+{
+	// sort islands in order of decreasing batch size
+	m_activeIslands.quickSort(IslandBatchSizeSortPredicate());
+
+	// merge small islands to satisfy minimum batch size
+	// find first small batch island
+	int destIslandIndex = m_activeIslands.size();
+	for (int i = 0; i < m_activeIslands.size(); ++i)
+	{
+		Island* island = m_activeIslands[i];
+		int batchSize = calcBatchCost(island);
+		if (batchSize < m_minimumSolverBatchSize)
+		{
+			destIslandIndex = i;
+			break;
+		}
+	}
+	int lastIndex = m_activeIslands.size() - 1;
+	while (destIslandIndex < lastIndex)
+	{
+		// merge islands from the back of the list
+		Island* island = m_activeIslands[destIslandIndex];
+		int numBodies = island->bodyArray.size();
+		int numManifolds = island->manifoldArray.size();
+		int numConstraints = island->constraintArray.size();
+		int firstIndex = lastIndex;
+		// figure out how many islands we want to merge and find out how many bodies, manifolds and constraints we will have
+		while (true)
+		{
+			Island* src = m_activeIslands[firstIndex];
+			numBodies += src->bodyArray.size();
+			numManifolds += src->manifoldArray.size();
+			numConstraints += src->constraintArray.size();
+			int batchCost = calcBatchCost(numBodies, numManifolds, numConstraints);
+			if (batchCost >= m_minimumSolverBatchSize)
+			{
+				break;
+			}
+			if (firstIndex - 1 == destIslandIndex)
+			{
+				break;
+			}
+			firstIndex--;
+		}
+		// reserve space for these pointers to minimize reallocation
+		island->bodyArray.reserve(numBodies);
+		island->manifoldArray.reserve(numManifolds);
+		island->constraintArray.reserve(numConstraints);
+		// merge islands
+		for (int i = firstIndex; i <= lastIndex; ++i)
+		{
+			island->append(*m_activeIslands[i]);
+		}
+		// shrink array to exclude the islands that were merged from
+		m_activeIslands.resize(firstIndex);
+		lastIndex = firstIndex - 1;
+		destIslandIndex++;
+	}
+}
+
+void btSimulationIslandManagerMt::solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams)
+{
+	btPersistentManifold** manifolds = island.manifoldArray.size() ? &island.manifoldArray[0] : NULL;
+	btTypedConstraint** constraintsPtr = island.constraintArray.size() ? &island.constraintArray[0] : NULL;
+	solver->solveGroup(&island.bodyArray[0],
+					   island.bodyArray.size(),
+					   manifolds,
+					   island.manifoldArray.size(),
+					   constraintsPtr,
+					   island.constraintArray.size(),
+					   *solverParams.m_solverInfo,
+					   solverParams.m_debugDrawer,
+					   solverParams.m_dispatcher);
+}
+
+void btSimulationIslandManagerMt::serialIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams)
+{
+	BT_PROFILE("serialIslandDispatch");
+	// serial dispatch
+	btAlignedObjectArray<Island*>& islands = *islandsPtr;
+	btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool;
+	for (int i = 0; i < islands.size(); ++i)
+	{
+		solveIsland(solver, *islands[i], solverParams);
+	}
+}
+
+struct UpdateIslandDispatcher : public btIParallelForBody
+{
+	btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& m_islandsPtr;
+	const btSimulationIslandManagerMt::SolverParams& m_solverParams;
+
+	UpdateIslandDispatcher(btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& islandsPtr, const btSimulationIslandManagerMt::SolverParams& solverParams)
+		: m_islandsPtr(islandsPtr), m_solverParams(solverParams)
+	{
+	}
+
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		btConstraintSolver* solver = m_solverParams.m_solverPool;
+		for (int i = iBegin; i < iEnd; ++i)
+		{
+			btSimulationIslandManagerMt::Island* island = m_islandsPtr[i];
+			btSimulationIslandManagerMt::solveIsland(solver, *island, m_solverParams);
+		}
+	}
+};
+
+void btSimulationIslandManagerMt::parallelIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams)
+{
+	BT_PROFILE("parallelIslandDispatch");
+	//
+	// if there are islands with many contacts, it may be faster to submit these
+	// large islands *serially* to a single parallel constraint solver, and then later
+	// submit the remaining smaller islands in parallel to multiple sequential solvers.
+	//
+	// Some task schedulers do not deal well with nested parallelFor loops. One implementation
+	// of OpenMP was actually slower than doing everything single-threaded. Intel TBB
+	// on the other hand, seems to do a pretty respectable job with it.
+	//
+	// When solving islands in parallel, the worst case performance happens when there
+	// is one very large island and then perhaps a smattering of very small
+	// islands -- one worker thread takes the large island and the remaining workers
+	// tear through the smaller islands and then sit idle waiting for the first worker
+	// to finish. Solving islands in parallel works best when there are numerous small
+	// islands, roughly equal in size.
+	//
+	// By contrast, the other approach -- the parallel constraint solver -- is only
+	// able to deliver a worthwhile speedup when the island is large. For smaller islands,
+	// it is difficult to extract a useful amount of parallelism -- the overhead of grouping
+	// the constraints into batches and sending the batches to worker threads can nullify
+	// any gains from parallelism.
+	//
+
+	UpdateIslandDispatcher dispatcher(*islandsPtr, solverParams);
+	// We take advantage of the fact the islands are sorted in order of decreasing size
+	int iBegin = 0;
+	if (solverParams.m_solverMt)
+	{
+		while (iBegin < islandsPtr->size())
+		{
+			btSimulationIslandManagerMt::Island* island = (*islandsPtr)[iBegin];
+			if (island->manifoldArray.size() < btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching)
+			{
+				// OK to submit the rest of the array in parallel
+				break;
+			}
+			// serial dispatch to parallel solver for large islands (if any)
+			solveIsland(solverParams.m_solverMt, *island, solverParams);
+			++iBegin;
+		}
+	}
+	// parallel dispatch to sequential solvers for rest
+	btParallelFor(iBegin, islandsPtr->size(), 1, dispatcher);
+}
+
+///@todo: this is random access, it can be walked 'cache friendly'!
+void btSimulationIslandManagerMt::buildAndProcessIslands(btDispatcher* dispatcher,
+														 btCollisionWorld* collisionWorld,
+														 btAlignedObjectArray<btTypedConstraint*>& constraints,
+														 const SolverParams& solverParams)
+{
+	BT_PROFILE("buildAndProcessIslands");
+	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+
+	buildIslands(dispatcher, collisionWorld);
+
+	if (!getSplitIslands())
+	{
+		btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer();
+		int maxNumManifolds = dispatcher->getNumManifolds();
+
+		for (int i = 0; i < maxNumManifolds; i++)
+		{
+			btPersistentManifold* manifold = manifolds[i];
+
+			const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
+			const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
+
+			///@todo: check sleeping conditions!
+			if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
+				((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
+			{
+				//kinematic objects don't merge islands, but wake up all connected objects
+				if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
+				{
+					if (colObj0->hasContactResponse())
+						colObj1->activate();
+				}
+				if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
+				{
+					if (colObj1->hasContactResponse())
+						colObj0->activate();
+				}
+			}
+		}
+		btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[0] : NULL;
+		btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool;
+		solver->solveGroup(&collisionObjects[0],
+						   collisionObjects.size(),
+						   manifolds,
+						   maxNumManifolds,
+						   constraintsPtr,
+						   constraints.size(),
+						   *solverParams.m_solverInfo,
+						   solverParams.m_debugDrawer,
+						   solverParams.m_dispatcher);
+	}
+	else
+	{
+		initIslandPools();
+
+		//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
+		addBodiesToIslands(collisionWorld);
+		addManifoldsToIslands(dispatcher);
+		addConstraintsToIslands(constraints);
+
+		// m_activeIslands array should now contain all non-sleeping Islands, and each Island should
+		// have all the necessary bodies, manifolds and constraints.
+
+		// if we want to merge islands with small batch counts,
+		if (m_minimumSolverBatchSize > 1)
+		{
+			mergeIslands();
+		}
+		// dispatch islands to solver
+		m_islandDispatch(&m_activeIslands, solverParams);
+	}
+}
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h b/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h
new file mode 100644
index 00000000000..ab73a899f14
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h
@@ -0,0 +1,112 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_SIMULATION_ISLAND_MANAGER_MT_H
+#define BT_SIMULATION_ISLAND_MANAGER_MT_H
+
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+
+class btTypedConstraint;
+class btConstraintSolver;
+struct btContactSolverInfo;
+class btIDebugDraw;
+
+///
+/// SimulationIslandManagerMt -- Multithread capable version of SimulationIslandManager
+///                       Splits the world up into islands which can be solved in parallel.
+///                       In order to solve islands in parallel, an IslandDispatch function
+///                       must be provided which will dispatch calls to multiple threads.
+///                       The amount of parallelism that can be achieved depends on the number
+///                       of islands. If only a single island exists, then no parallelism is
+///                       possible.
+///
+class btSimulationIslandManagerMt : public btSimulationIslandManager
+{
+public:
+	struct Island
+	{
+		// a simulation island consisting of bodies, manifolds and constraints,
+		// to be passed into a constraint solver.
+		btAlignedObjectArray<btCollisionObject*> bodyArray;
+		btAlignedObjectArray<btPersistentManifold*> manifoldArray;
+		btAlignedObjectArray<btTypedConstraint*> constraintArray;
+		int id;  // island id
+		bool isSleeping;
+
+		void append(const Island& other);  // add bodies, manifolds, constraints to my own
+	};
+	struct SolverParams
+	{
+		btConstraintSolver* m_solverPool;
+		btConstraintSolver* m_solverMt;
+		btContactSolverInfo* m_solverInfo;
+		btIDebugDraw* m_debugDrawer;
+		btDispatcher* m_dispatcher;
+	};
+	static void solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams);
+
+	typedef void (*IslandDispatchFunc)(btAlignedObjectArray<Island*>* islands, const SolverParams& solverParams);
+	static void serialIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams);
+	static void parallelIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams);
+
+protected:
+	btAlignedObjectArray<Island*> m_allocatedIslands;    // owner of all Islands
+	btAlignedObjectArray<Island*> m_activeIslands;       // islands actively in use
+	btAlignedObjectArray<Island*> m_freeIslands;         // islands ready to be reused
+	btAlignedObjectArray<Island*> m_lookupIslandFromId;  // big lookup table to map islandId to Island pointer
+	Island* m_batchIsland;
+	int m_minimumSolverBatchSize;
+	int m_batchIslandMinBodyCount;
+	IslandDispatchFunc m_islandDispatch;
+
+	Island* getIsland(int id);
+	virtual Island* allocateIsland(int id, int numBodies);
+	virtual void initIslandPools();
+	virtual void addBodiesToIslands(btCollisionWorld* collisionWorld);
+	virtual void addManifoldsToIslands(btDispatcher* dispatcher);
+	virtual void addConstraintsToIslands(btAlignedObjectArray<btTypedConstraint*>& constraints);
+	virtual void mergeIslands();
+
+public:
+	btSimulationIslandManagerMt();
+	virtual ~btSimulationIslandManagerMt();
+
+	virtual void buildAndProcessIslands(btDispatcher* dispatcher,
+										btCollisionWorld* collisionWorld,
+										btAlignedObjectArray<btTypedConstraint*>& constraints,
+										const SolverParams& solverParams);
+
+	virtual void buildIslands(btDispatcher* dispatcher, btCollisionWorld* colWorld);
+
+	int getMinimumSolverBatchSize() const
+	{
+		return m_minimumSolverBatchSize;
+	}
+	void setMinimumSolverBatchSize(int sz)
+	{
+		m_minimumSolverBatchSize = sz;
+	}
+	IslandDispatchFunc getIslandDispatchFunction() const
+	{
+		return m_islandDispatch;
+	}
+	// allow users to set their own dispatch function for multithreaded dispatch
+	void setIslandDispatchFunction(IslandDispatchFunc func)
+	{
+		m_islandDispatch = func;
+	}
+};
+
+#endif  //BT_SIMULATION_ISLAND_MANAGER_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp
index aea8c683732..9862bd2e228 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -21,37 +21,38 @@
  
  */
 
-
 #include "btMultiBody.h"
 #include "btMultiBodyLink.h"
 #include "btMultiBodyLinkCollider.h"
 #include "btMultiBodyJointFeedback.h"
 #include "LinearMath/btTransformUtil.h"
 #include "LinearMath/btSerializer.h"
-// #define INCLUDE_GYRO_TERM 
+//#include "Bullet3Common/b3Logging.h"
+// #define INCLUDE_GYRO_TERM
 
-///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals
-bool gJointFeedbackInWorldSpace = false;
-bool gJointFeedbackInJointFrame = false;
 
-namespace {
-    const btScalar SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m^2 s^-2)
-    const btScalar SLEEP_TIMEOUT = btScalar(2);     // in seconds
+namespace
+{
+const btScalar SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m^2 s^-2)
+const btScalar SLEEP_TIMEOUT = btScalar(2);     // in seconds
+}  // namespace
+
+void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
+	const btVector3 &displacement,       // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
+	const btVector3 &top_in,             // top part of input vector
+	const btVector3 &bottom_in,          // bottom part of input vector
+	btVector3 &top_out,                  // top part of output vector
+	btVector3 &bottom_out)               // bottom part of output vector
+{
+	top_out = rotation_matrix * top_in;
+	bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
 }
 
-namespace {
-    void SpatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
-                          const btVector3 &displacement,     // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
-                          const btVector3 &top_in,       // top part of input vector
-                          const btVector3 &bottom_in,    // bottom part of input vector
-                          btVector3 &top_out,         // top part of output vector
-                          btVector3 &bottom_out)      // bottom part of output vector
-    {
-        top_out = rotation_matrix * top_in;
-        bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
-    }
+namespace
+{
 
-/*
+
+#if 0
     void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
                                  const btVector3 &displacement,
                                  const btVector3 &top_in,
@@ -81,52 +82,65 @@ namespace {
 		top_out = a_top.cross(b_top);
 		bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom);
 	}
-*/
-}
+#endif
 
+}  // namespace
 
 //
 // Implementation of class btMultiBody
 //
 
 btMultiBody::btMultiBody(int n_links,
-                     btScalar mass,
-                     const btVector3 &inertia,
-                     bool fixedBase,
-                     bool canSleep,
-		     bool /*deprecatedUseMultiDof*/)
-    : 
-    	m_baseCollider(0),
-		m_baseName(0),
-    	m_basePos(0,0,0),
-    	m_baseQuat(0, 0, 0, 1),
-      m_baseMass(mass),
-      m_baseInertia(inertia),
-    
-		m_fixedBase(fixedBase),
-		m_awake(true),
-		m_canSleep(canSleep),
-		m_sleepTimer(0),
-		
-		m_linearDamping(0.04f),
-		m_angularDamping(0.04f),
-		m_useGyroTerm(true),
-			m_maxAppliedImpulse(1000.f),
-		m_maxCoordinateVelocity(100.f),
-			m_hasSelfCollision(true),		
-		__posUpdated(false),
-			m_dofCount(0),
-		m_posVarCnt(0),
-		m_useRK4(false), 	
-		m_useGlobalVelocities(false),
-		m_internalNeedsJointFeedback(false)
+						 btScalar mass,
+						 const btVector3 &inertia,
+						 bool fixedBase,
+						 bool canSleep,
+						 bool /*deprecatedUseMultiDof*/)
+	: m_baseCollider(0),
+	  m_baseName(0),
+	  m_basePos(0, 0, 0),
+	  m_baseQuat(0, 0, 0, 1),
+      m_basePos_interpolate(0, 0, 0),
+      m_baseQuat_interpolate(0, 0, 0, 1),
+	  m_baseMass(mass),
+	  m_baseInertia(inertia),
+
+	  m_fixedBase(fixedBase),
+	  m_awake(true),
+	  m_canSleep(canSleep),
+	  m_canWakeup(true),
+	  m_sleepTimer(0),
+	  m_userObjectPointer(0),
+	  m_userIndex2(-1),
+	  m_userIndex(-1),
+	  m_companionId(-1),
+	  m_linearDamping(0.04f),
+	  m_angularDamping(0.04f),
+	  m_useGyroTerm(true),
+	  m_maxAppliedImpulse(1000.f),
+	  m_maxCoordinateVelocity(100.f),
+	  m_hasSelfCollision(true),
+	  __posUpdated(false),
+	  m_dofCount(0),
+	  m_posVarCnt(0),
+	  m_useRK4(false),
+	  m_useGlobalVelocities(false),
+	  m_internalNeedsJointFeedback(false)
 {
+	m_cachedInertiaTopLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	m_cachedInertiaTopRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	m_cachedInertiaLowerLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	m_cachedInertiaLowerRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
+	m_cachedInertiaValid = false;
+
 	m_links.resize(n_links);
 	m_matrixBuf.resize(n_links + 1);
 
+	m_baseForce.setValue(0, 0, 0);
+	m_baseTorque.setValue(0, 0, 0);
 
-    m_baseForce.setValue(0, 0, 0);
-    m_baseTorque.setValue(0, 0, 0);
+	clearConstraintForces();
+	clearForcesAndTorques();
 }
 
 btMultiBody::~btMultiBody()
@@ -134,129 +148,125 @@ btMultiBody::~btMultiBody()
 }
 
 void btMultiBody::setupFixed(int i,
-						   btScalar mass,
-						   const btVector3 &inertia,
-						   int parent,
-						   const btQuaternion &rotParentToThis,
-						   const btVector3 &parentComToThisPivotOffset,
-                           const btVector3 &thisPivotToThisComOffset, bool /*deprecatedDisableParentCollision*/)
+							 btScalar mass,
+							 const btVector3 &inertia,
+							 int parent,
+							 const btQuaternion &rotParentToThis,
+							 const btVector3 &parentComToThisPivotOffset,
+							 const btVector3 &thisPivotToThisComOffset, bool /*deprecatedDisableParentCollision*/)
 {
-	
 	m_links[i].m_mass = mass;
-    m_links[i].m_inertiaLocal = inertia;
-    m_links[i].m_parent = parent;
-    m_links[i].m_zeroRotParentToThis = rotParentToThis;
+	m_links[i].m_inertiaLocal = inertia;
+	m_links[i].m_parent = parent;
+	m_links[i].setAxisTop(0, 0., 0., 0.);
+	m_links[i].setAxisBottom(0, btVector3(0, 0, 0));
+	m_links[i].m_zeroRotParentToThis = rotParentToThis;
 	m_links[i].m_dVector = thisPivotToThisComOffset;
-    m_links[i].m_eVector = parentComToThisPivotOffset;    
+	m_links[i].m_eVector = parentComToThisPivotOffset;
 
 	m_links[i].m_jointType = btMultibodyLink::eFixed;
 	m_links[i].m_dofCount = 0;
 	m_links[i].m_posVarCount = 0;
 
-	m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;    
-	
+	m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+
 	m_links[i].updateCacheMultiDof();
 
 	updateLinksDofOffsets();
-
 }
 
-
 void btMultiBody::setupPrismatic(int i,
-                               btScalar mass,
-                               const btVector3 &inertia,
-                               int parent,
-                               const btQuaternion &rotParentToThis,
-                               const btVector3 &jointAxis,
-                               const btVector3 &parentComToThisPivotOffset,
-							   const btVector3 &thisPivotToThisComOffset,
-							   bool disableParentCollision)
+								 btScalar mass,
+								 const btVector3 &inertia,
+								 int parent,
+								 const btQuaternion &rotParentToThis,
+								 const btVector3 &jointAxis,
+								 const btVector3 &parentComToThisPivotOffset,
+								 const btVector3 &thisPivotToThisComOffset,
+								 bool disableParentCollision)
 {
 	m_dofCount += 1;
 	m_posVarCnt += 1;
-	
-    m_links[i].m_mass = mass;
-    m_links[i].m_inertiaLocal = inertia;
-    m_links[i].m_parent = parent;
-    m_links[i].m_zeroRotParentToThis = rotParentToThis;
-    m_links[i].setAxisTop(0, 0., 0., 0.);
-    m_links[i].setAxisBottom(0, jointAxis);
-    m_links[i].m_eVector = parentComToThisPivotOffset;
+
+	m_links[i].m_mass = mass;
+	m_links[i].m_inertiaLocal = inertia;
+	m_links[i].m_parent = parent;
+	m_links[i].m_zeroRotParentToThis = rotParentToThis;
+	m_links[i].setAxisTop(0, 0., 0., 0.);
+	m_links[i].setAxisBottom(0, jointAxis);
+	m_links[i].m_eVector = parentComToThisPivotOffset;
 	m_links[i].m_dVector = thisPivotToThisComOffset;
-    m_links[i].m_cachedRotParentToThis = rotParentToThis;
+	m_links[i].m_cachedRotParentToThis = rotParentToThis;
 
 	m_links[i].m_jointType = btMultibodyLink::ePrismatic;
 	m_links[i].m_dofCount = 1;
-	m_links[i].m_posVarCount = 1;	
+	m_links[i].m_posVarCount = 1;
 	m_links[i].m_jointPos[0] = 0.f;
 	m_links[i].m_jointTorque[0] = 0.f;
 
 	if (disableParentCollision)
-		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
 	//
-	
+
 	m_links[i].updateCacheMultiDof();
-	
+
 	updateLinksDofOffsets();
 }
 
 void btMultiBody::setupRevolute(int i,
-                              btScalar mass,
-                              const btVector3 &inertia,
-                              int parent,
-                              const btQuaternion &rotParentToThis,
-                              const btVector3 &jointAxis,
-                              const btVector3 &parentComToThisPivotOffset,
-                              const btVector3 &thisPivotToThisComOffset,
-							  bool disableParentCollision)
+								btScalar mass,
+								const btVector3 &inertia,
+								int parent,
+								const btQuaternion &rotParentToThis,
+								const btVector3 &jointAxis,
+								const btVector3 &parentComToThisPivotOffset,
+								const btVector3 &thisPivotToThisComOffset,
+								bool disableParentCollision)
 {
 	m_dofCount += 1;
 	m_posVarCnt += 1;
-	
-    m_links[i].m_mass = mass;
-    m_links[i].m_inertiaLocal = inertia;
-    m_links[i].m_parent = parent;
-    m_links[i].m_zeroRotParentToThis = rotParentToThis;
-    m_links[i].setAxisTop(0, jointAxis);
-    m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));
-    m_links[i].m_dVector = thisPivotToThisComOffset;
-    m_links[i].m_eVector = parentComToThisPivotOffset;
+
+	m_links[i].m_mass = mass;
+	m_links[i].m_inertiaLocal = inertia;
+	m_links[i].m_parent = parent;
+	m_links[i].m_zeroRotParentToThis = rotParentToThis;
+	m_links[i].setAxisTop(0, jointAxis);
+	m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));
+	m_links[i].m_dVector = thisPivotToThisComOffset;
+	m_links[i].m_eVector = parentComToThisPivotOffset;
 
 	m_links[i].m_jointType = btMultibodyLink::eRevolute;
 	m_links[i].m_dofCount = 1;
-	m_links[i].m_posVarCount = 1;	
+	m_links[i].m_posVarCount = 1;
 	m_links[i].m_jointPos[0] = 0.f;
 	m_links[i].m_jointTorque[0] = 0.f;
 
 	if (disableParentCollision)
-		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
-    //
+		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+	//
 	m_links[i].updateCacheMultiDof();
 	//
 	updateLinksDofOffsets();
 }
 
-
-
 void btMultiBody::setupSpherical(int i,
-						   btScalar mass,
-						   const btVector3 &inertia,
-						   int parent,
-						   const btQuaternion &rotParentToThis,
-						   const btVector3 &parentComToThisPivotOffset,
-						   const btVector3 &thisPivotToThisComOffset,
-						   bool disableParentCollision)
+								 btScalar mass,
+								 const btVector3 &inertia,
+								 int parent,
+								 const btQuaternion &rotParentToThis,
+								 const btVector3 &parentComToThisPivotOffset,
+								 const btVector3 &thisPivotToThisComOffset,
+								 bool disableParentCollision)
 {
-	
 	m_dofCount += 3;
 	m_posVarCnt += 4;
 
 	m_links[i].m_mass = mass;
-    m_links[i].m_inertiaLocal = inertia;
-    m_links[i].m_parent = parent;
-    m_links[i].m_zeroRotParentToThis = rotParentToThis;    
-    m_links[i].m_dVector = thisPivotToThisComOffset;
-    m_links[i].m_eVector = parentComToThisPivotOffset;    
+	m_links[i].m_inertiaLocal = inertia;
+	m_links[i].m_parent = parent;
+	m_links[i].m_zeroRotParentToThis = rotParentToThis;
+	m_links[i].m_dVector = thisPivotToThisComOffset;
+	m_links[i].m_eVector = parentComToThisPivotOffset;
 
 	m_links[i].m_jointType = btMultibodyLink::eSpherical;
 	m_links[i].m_dofCount = 3;
@@ -267,323 +277,369 @@ void btMultiBody::setupSpherical(int i,
 	m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
 	m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
 	m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
-	m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; m_links[i].m_jointPos[3] = 1.f;
+	m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
+	m_links[i].m_jointPos[3] = 1.f;
 	m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
 
-
 	if (disableParentCollision)
-		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;    
+		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
 	//
-	m_links[i].updateCacheMultiDof();	
+	m_links[i].updateCacheMultiDof();
 	//
 	updateLinksDofOffsets();
 }
 
 void btMultiBody::setupPlanar(int i,
-						   btScalar mass,
-						   const btVector3 &inertia,
-						   int parent,
-						   const btQuaternion &rotParentToThis,
-						   const btVector3 &rotationAxis,
-						   const btVector3 &parentComToThisComOffset,						   
-						   bool disableParentCollision)
+							  btScalar mass,
+							  const btVector3 &inertia,
+							  int parent,
+							  const btQuaternion &rotParentToThis,
+							  const btVector3 &rotationAxis,
+							  const btVector3 &parentComToThisComOffset,
+							  bool disableParentCollision)
 {
-	
 	m_dofCount += 3;
 	m_posVarCnt += 3;
 
 	m_links[i].m_mass = mass;
-    m_links[i].m_inertiaLocal = inertia;
-    m_links[i].m_parent = parent;
-    m_links[i].m_zeroRotParentToThis = rotParentToThis;    
+	m_links[i].m_inertiaLocal = inertia;
+	m_links[i].m_parent = parent;
+	m_links[i].m_zeroRotParentToThis = rotParentToThis;
 	m_links[i].m_dVector.setZero();
-    m_links[i].m_eVector = parentComToThisComOffset;
+	m_links[i].m_eVector = parentComToThisComOffset;
 
 	//
-	static btVector3 vecNonParallelToRotAxis(1, 0, 0);
-	if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
+	btVector3 vecNonParallelToRotAxis(1, 0, 0);
+	if (rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
 		vecNonParallelToRotAxis.setValue(0, 1, 0);
 	//
 
 	m_links[i].m_jointType = btMultibodyLink::ePlanar;
 	m_links[i].m_dofCount = 3;
 	m_links[i].m_posVarCount = 3;
-	btVector3 n=rotationAxis.normalized();
-	m_links[i].setAxisTop(0, n[0],n[1],n[2]);
-	m_links[i].setAxisTop(1,0,0,0);
-	m_links[i].setAxisTop(2,0,0,0);
-	m_links[i].setAxisBottom(0,0,0,0);
+	btVector3 n = rotationAxis.normalized();
+	m_links[i].setAxisTop(0, n[0], n[1], n[2]);
+	m_links[i].setAxisTop(1, 0, 0, 0);
+	m_links[i].setAxisTop(2, 0, 0, 0);
+	m_links[i].setAxisBottom(0, 0, 0, 0);
 	btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
-	m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]);
+	m_links[i].setAxisBottom(1, cr[0], cr[1], cr[2]);
 	cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
-	m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]);
+	m_links[i].setAxisBottom(2, cr[0], cr[1], cr[2]);
 	m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
 	m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
 
 	if (disableParentCollision)
-		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
-    //
+		m_links[i].m_flags |= BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
+	//
 	m_links[i].updateCacheMultiDof();
 	//
 	updateLinksDofOffsets();
+
+	m_links[i].setAxisBottom(1, m_links[i].getAxisBottom(1).normalized());
+	m_links[i].setAxisBottom(2, m_links[i].getAxisBottom(2).normalized());
 }
 
 void btMultiBody::finalizeMultiDof()
 {
 	m_deltaV.resize(0);
 	m_deltaV.resize(6 + m_dofCount);
-	m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount);			//m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
-	m_vectorBuf.resize(2 * m_dofCount);													//two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
-
+    m_splitV.resize(0);
+    m_splitV.resize(6 + m_dofCount);
+	m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount);  //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
+	m_vectorBuf.resize(2 * m_dofCount);                                           //two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
+	m_matrixBuf.resize(m_links.size() + 1);
+	for (int i = 0; i < m_vectorBuf.size(); i++)
+	{
+		m_vectorBuf[i].setValue(0, 0, 0);
+	}
 	updateLinksDofOffsets();
 }
-	
-int btMultiBody::getParent(int i) const
+
+int btMultiBody::getParent(int link_num) const
 {
-    return m_links[i].m_parent;
+	return m_links[link_num].m_parent;
 }
 
 btScalar btMultiBody::getLinkMass(int i) const
 {
-    return m_links[i].m_mass;
+	return m_links[i].m_mass;
 }
 
-const btVector3 & btMultiBody::getLinkInertia(int i) const
+const btVector3 &btMultiBody::getLinkInertia(int i) const
 {
-    return m_links[i].m_inertiaLocal;
+	return m_links[i].m_inertiaLocal;
 }
 
 btScalar btMultiBody::getJointPos(int i) const
 {
-    return m_links[i].m_jointPos[0];
+	return m_links[i].m_jointPos[0];
 }
 
 btScalar btMultiBody::getJointVel(int i) const
 {
-    return m_realBuf[6 + m_links[i].m_dofOffset];
+	return m_realBuf[6 + m_links[i].m_dofOffset];
 }
 
-btScalar * btMultiBody::getJointPosMultiDof(int i)
+btScalar *btMultiBody::getJointPosMultiDof(int i)
 {
 	return &m_links[i].m_jointPos[0];
 }
 
-btScalar * btMultiBody::getJointVelMultiDof(int i)
+btScalar *btMultiBody::getJointVelMultiDof(int i)
 {
 	return &m_realBuf[6 + m_links[i].m_dofOffset];
 }
 
-const btScalar * btMultiBody::getJointPosMultiDof(int i) const 
+const btScalar *btMultiBody::getJointPosMultiDof(int i) const
 {
 	return &m_links[i].m_jointPos[0];
 }
 
-const btScalar * btMultiBody::getJointVelMultiDof(int i) const 
+const btScalar *btMultiBody::getJointVelMultiDof(int i) const
 {
 	return &m_realBuf[6 + m_links[i].m_dofOffset];
 }
 
-
 void btMultiBody::setJointPos(int i, btScalar q)
 {
-    m_links[i].m_jointPos[0] = q;
-    m_links[i].updateCacheMultiDof();
+	m_links[i].m_jointPos[0] = q;
+	m_links[i].updateCacheMultiDof();
+}
+
+
+void btMultiBody::setJointPosMultiDof(int i, const double *q)
+{
+	for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
+		m_links[i].m_jointPos[pos] = (btScalar)q[pos];
+
+	m_links[i].updateCacheMultiDof();
 }
 
-void btMultiBody::setJointPosMultiDof(int i, btScalar *q)
+void btMultiBody::setJointPosMultiDof(int i, const float *q)
 {
-	for(int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
-		m_links[i].m_jointPos[pos] = q[pos];
+	for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
+		m_links[i].m_jointPos[pos] = (btScalar)q[pos];
 
-    m_links[i].updateCacheMultiDof();
+	m_links[i].updateCacheMultiDof();
 }
 
+
+
 void btMultiBody::setJointVel(int i, btScalar qdot)
 {
-    m_realBuf[6 + m_links[i].m_dofOffset] = qdot;
+	m_realBuf[6 + m_links[i].m_dofOffset] = qdot;
+}
+
+void btMultiBody::setJointVelMultiDof(int i, const double *qdot)
+{
+	for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof];
 }
 
-void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot)
+void btMultiBody::setJointVelMultiDof(int i, const float* qdot)
 {
-	for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof];
+	for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof];
 }
 
-const btVector3 & btMultiBody::getRVector(int i) const
+const btVector3 &btMultiBody::getRVector(int i) const
 {
-    return m_links[i].m_cachedRVector;
+	return m_links[i].m_cachedRVector;
 }
 
-const btQuaternion & btMultiBody::getParentToLocalRot(int i) const
+const btQuaternion &btMultiBody::getParentToLocalRot(int i) const
 {
-    return m_links[i].m_cachedRotParentToThis;
+	return m_links[i].m_cachedRotParentToThis;
+}
+
+const btVector3 &btMultiBody::getInterpolateRVector(int i) const
+{
+    return m_links[i].m_cachedRVector_interpolate;
+}
+
+const btQuaternion &btMultiBody::getInterpolateParentToLocalRot(int i) const
+{
+    return m_links[i].m_cachedRotParentToThis_interpolate;
 }
 
 btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
 {
-    btVector3 result = local_pos;
-    while (i != -1) {
-        // 'result' is in frame i. transform it to frame parent(i)
-        result += getRVector(i);
-        result = quatRotate(getParentToLocalRot(i).inverse(),result);
-        i = getParent(i);
-    }
+	btAssert(i >= -1);
+	btAssert(i < m_links.size());
+	if ((i < -1) || (i >= m_links.size()))
+	{
+		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
+	}
+
+	btVector3 result = local_pos;
+	while (i != -1)
+	{
+		// 'result' is in frame i. transform it to frame parent(i)
+		result += getRVector(i);
+		result = quatRotate(getParentToLocalRot(i).inverse(), result);
+		i = getParent(i);
+	}
 
-    // 'result' is now in the base frame. transform it to world frame
-    result = quatRotate(getWorldToBaseRot().inverse() ,result);
-    result += getBasePos();
+	// 'result' is now in the base frame. transform it to world frame
+	result = quatRotate(getWorldToBaseRot().inverse(), result);
+	result += getBasePos();
 
-    return result;
+	return result;
 }
 
 btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
 {
-    if (i == -1) {
-        // world to base
-        return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos()));
-    } else {
-        // find position in parent frame, then transform to current frame
-        return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
-    }
+	btAssert(i >= -1);
+	btAssert(i < m_links.size());
+	if ((i < -1) || (i >= m_links.size()))
+	{
+		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
+	}
+
+	if (i == -1)
+	{
+		// world to base
+		return quatRotate(getWorldToBaseRot(), (world_pos - getBasePos()));
+	}
+	else
+	{
+		// find position in parent frame, then transform to current frame
+		return quatRotate(getParentToLocalRot(i), worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
+	}
 }
 
 btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
 {
-    btVector3 result = local_dir;
-    while (i != -1) {
-        result = quatRotate(getParentToLocalRot(i).inverse() , result);
-        i = getParent(i);
-    }
-    result = quatRotate(getWorldToBaseRot().inverse() , result);
-    return result;
+	btAssert(i >= -1);
+	btAssert(i < m_links.size());
+	if ((i < -1) || (i >= m_links.size()))
+	{
+		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
+	}
+
+	btVector3 result = local_dir;
+	while (i != -1)
+	{
+		result = quatRotate(getParentToLocalRot(i).inverse(), result);
+		i = getParent(i);
+	}
+	result = quatRotate(getWorldToBaseRot().inverse(), result);
+	return result;
 }
 
 btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
 {
-    if (i == -1) {
-        return quatRotate(getWorldToBaseRot(), world_dir);
-    } else {
-        return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir));
-    }
+	btAssert(i >= -1);
+	btAssert(i < m_links.size());
+	if ((i < -1) || (i >= m_links.size()))
+	{
+		return btVector3(SIMD_INFINITY, SIMD_INFINITY, SIMD_INFINITY);
+	}
+
+	if (i == -1)
+	{
+		return quatRotate(getWorldToBaseRot(), world_dir);
+	}
+	else
+	{
+		return quatRotate(getParentToLocalRot(i), worldDirToLocal(getParent(i), world_dir));
+	}
 }
 
-void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
+btMatrix3x3 btMultiBody::localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
 {
-	int num_links = getNumLinks();
-    // Calculates the velocities of each link (and the base) in its local frame
-    omega[0] = quatRotate(m_baseQuat ,getBaseOmega());
-    vel[0] = quatRotate(m_baseQuat ,getBaseVel());
-    
-    for (int i = 0; i < num_links; ++i) {
-        const int parent = m_links[i].m_parent;
-
-        // transform parent vel into this frame, store in omega[i+1], vel[i+1]
-        SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector,
-                         omega[parent+1], vel[parent+1],
-                         omega[i+1], vel[i+1]);
-
-        // now add qidot * shat_i
-        omega[i+1] += getJointVel(i) * m_links[i].getAxisTop(0);
-        vel[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
-    }
+	btMatrix3x3 result = local_frame;
+	btVector3 frameInWorld0 = localDirToWorld(i, local_frame.getColumn(0));
+	btVector3 frameInWorld1 = localDirToWorld(i, local_frame.getColumn(1));
+	btVector3 frameInWorld2 = localDirToWorld(i, local_frame.getColumn(2));
+	result.setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
+	return result;
 }
 
-btScalar btMultiBody::getKineticEnergy() const
+void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
 {
 	int num_links = getNumLinks();
-    // TODO: would be better not to allocate memory here
-    btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
-	btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
-    compTreeLinkVelocities(&omega[0], &vel[0]);
-
-    // we will do the factor of 0.5 at the end
-    btScalar result = m_baseMass * vel[0].dot(vel[0]);
-    result += omega[0].dot(m_baseInertia * omega[0]);
-    
-    for (int i = 0; i < num_links; ++i) {
-        result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
-        result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]);
-    }
+	// Calculates the velocities of each link (and the base) in its local frame
+	const btQuaternion& base_rot = getWorldToBaseRot();
+	omega[0] = quatRotate(base_rot, getBaseOmega());
+	vel[0] = quatRotate(base_rot, getBaseVel());
 
-    return 0.5f * result;
-}
+	for (int i = 0; i < num_links; ++i)
+	{
+		const btMultibodyLink& link = getLink(i);
+		const int parent = link.m_parent;
 
-btVector3 btMultiBody::getAngularMomentum() const
-{
-	int num_links = getNumLinks();
-    // TODO: would be better not to allocate memory here
-    btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
-	btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
-    btAlignedObjectArray<btQuaternion> rot_from_world;rot_from_world.resize(num_links+1);
-    compTreeLinkVelocities(&omega[0], &vel[0]);
-
-    rot_from_world[0] = m_baseQuat;
-    btVector3 result = quatRotate(rot_from_world[0].inverse() , (m_baseInertia * omega[0]));
-
-    for (int i = 0; i < num_links; ++i) {
-        rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1];
-        result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
-    }
+		// transform parent vel into this frame, store in omega[i+1], vel[i+1]
+		spatialTransform(btMatrix3x3(link.m_cachedRotParentToThis), link.m_cachedRVector,
+			omega[parent + 1], vel[parent + 1],
+			omega[i + 1], vel[i + 1]);
 
-    return result;
+		// now add qidot * shat_i
+		const btScalar* jointVel = getJointVelMultiDof(i);
+		for (int dof = 0; dof < link.m_dofCount; ++dof)
+		{
+			omega[i + 1] += jointVel[dof] * link.getAxisTop(dof);
+			vel[i + 1] += jointVel[dof] * link.getAxisBottom(dof);
+		}
+	}
 }
 
+
 void btMultiBody::clearConstraintForces()
 {
 	m_baseConstraintForce.setValue(0, 0, 0);
 	m_baseConstraintTorque.setValue(0, 0, 0);
 
-
-    for (int i = 0; i < getNumLinks(); ++i) {
-        m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
-        m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
-    }
+	for (int i = 0; i < getNumLinks(); ++i)
+	{
+		m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
+		m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
+	}
 }
 void btMultiBody::clearForcesAndTorques()
 {
-    m_baseForce.setValue(0, 0, 0);
-    m_baseTorque.setValue(0, 0, 0);
+	m_baseForce.setValue(0, 0, 0);
+	m_baseTorque.setValue(0, 0, 0);
 
-	
-    for (int i = 0; i < getNumLinks(); ++i) {
-        m_links[i].m_appliedForce.setValue(0, 0, 0);
-        m_links[i].m_appliedTorque.setValue(0, 0, 0);
+	for (int i = 0; i < getNumLinks(); ++i)
+	{
+		m_links[i].m_appliedForce.setValue(0, 0, 0);
+		m_links[i].m_appliedTorque.setValue(0, 0, 0);
 		m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = m_links[i].m_jointTorque[3] = m_links[i].m_jointTorque[4] = m_links[i].m_jointTorque[5] = 0.f;
-    }
+	}
 }
 
 void btMultiBody::clearVelocities()
 {
-	for (int i = 0; i < 6 + getNumLinks(); ++i) 
+	for (int i = 0; i < 6 + getNumDofs(); ++i)
 	{
 		m_realBuf[i] = 0.f;
 	}
 }
 void btMultiBody::addLinkForce(int i, const btVector3 &f)
 {
-    m_links[i].m_appliedForce += f;
+	m_links[i].m_appliedForce += f;
 }
 
 void btMultiBody::addLinkTorque(int i, const btVector3 &t)
 {
-    m_links[i].m_appliedTorque += t;
+	m_links[i].m_appliedTorque += t;
 }
 
 void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f)
 {
-    m_links[i].m_appliedConstraintForce += f;
+	m_links[i].m_appliedConstraintForce += f;
 }
 
 void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t)
 {
-    m_links[i].m_appliedConstraintTorque += t;
+	m_links[i].m_appliedConstraintTorque += t;
 }
 
-
-
 void btMultiBody::addJointTorque(int i, btScalar Q)
 {
-    m_links[i].m_jointTorque[0] += Q;
+	m_links[i].m_jointTorque[0] += Q;
 }
 
 void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q)
@@ -593,70 +649,72 @@ void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q)
 
 void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q)
 {
-	for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+	for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		m_links[i].m_jointTorque[dof] = Q[dof];
 }
 
-const btVector3 & btMultiBody::getLinkForce(int i) const
+const btVector3 &btMultiBody::getLinkForce(int i) const
 {
-    return m_links[i].m_appliedForce;
+	return m_links[i].m_appliedForce;
 }
 
-const btVector3 & btMultiBody::getLinkTorque(int i) const
+const btVector3 &btMultiBody::getLinkTorque(int i) const
 {
-    return m_links[i].m_appliedTorque;
+	return m_links[i].m_appliedTorque;
 }
 
 btScalar btMultiBody::getJointTorque(int i) const
 {
-    return m_links[i].m_jointTorque[0];
+	return m_links[i].m_jointTorque[0];
 }
 
-btScalar * btMultiBody::getJointTorqueMultiDof(int i)
+btScalar *btMultiBody::getJointTorqueMultiDof(int i)
 {
-    return &m_links[i].m_jointTorque[0];
+	return &m_links[i].m_jointTorque[0];
 }
 
-inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1)				//renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross?
+inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)  //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross?
 {
-		btVector3 row0 = btVector3( 
-			v0.x() * v1.x(),
-			v0.x() * v1.y(),
-			v0.x() * v1.z());
-		btVector3 row1 = btVector3( 
-			v0.y() * v1.x(),
-			v0.y() * v1.y(),
-			v0.y() * v1.z());
-		btVector3 row2 = btVector3( 
-			v0.z() * v1.x(),
-			v0.z() * v1.y(),
-			v0.z() * v1.z());
-
-        btMatrix3x3 m(row0[0],row0[1],row0[2],
-						row1[0],row1[1],row1[2],
-						row2[0],row2[1],row2[2]);
-		return m;
+	btVector3 row0 = btVector3(
+		v0.x() * v1.x(),
+		v0.x() * v1.y(),
+		v0.x() * v1.z());
+	btVector3 row1 = btVector3(
+		v0.y() * v1.x(),
+		v0.y() * v1.y(),
+		v0.y() * v1.z());
+	btVector3 row2 = btVector3(
+		v0.z() * v1.x(),
+		v0.z() * v1.y(),
+		v0.z() * v1.z());
+
+	btMatrix3x3 m(row0[0], row0[1], row0[2],
+				  row1[0], row1[1], row1[2],
+				  row2[0], row2[1], row2[2]);
+	return m;
 }
 
 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
 //
 
 void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
-                               btAlignedObjectArray<btScalar> &scratch_r,
-                               btAlignedObjectArray<btVector3> &scratch_v,
-                               btAlignedObjectArray<btMatrix3x3> &scratch_m,
-				bool isConstraintPass)
+    btAlignedObjectArray<btScalar> &scratch_r,
+    btAlignedObjectArray<btVector3> &scratch_v,
+    btAlignedObjectArray<btMatrix3x3> &scratch_m,
+	bool isConstraintPass,
+	bool jointFeedbackInWorldSpace,
+	bool jointFeedbackInJointFrame)
 {
-    // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
-    // and the base linear & angular accelerations.
+	// Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
+	// and the base linear & angular accelerations.
 
-    // We apply damping forces in this routine as well as any external forces specified by the 
-    // caller (via addBaseForce etc).
+	// We apply damping forces in this routine as well as any external forces specified by the
+	// caller (via addBaseForce etc).
+
+	// output should point to an array of 6 + num_links reals.
+	// Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
+	// num_links joint acceleration values.
 
-    // output should point to an array of 6 + num_links reals.
-    // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
-    // num_links joint acceleration values.
-    
 	// We added support for multi degree of freedom (multi dof) joints.
 	// In addition we also can compute the joint reaction forces. This is performed in a second pass,
 	// so that we can include the effect of the constraint solver forces (computed in the PGS LCP solver)
@@ -665,96 +723,96 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 
 	int num_links = getNumLinks();
 
-    const btScalar DAMPING_K1_LINEAR = m_linearDamping;
+	const btScalar DAMPING_K1_LINEAR = m_linearDamping;
 	const btScalar DAMPING_K2_LINEAR = m_linearDamping;
 
 	const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
-	const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
+	const btScalar DAMPING_K2_ANGULAR = m_angularDamping;
 
-    btVector3 base_vel = getBaseVel();
-    btVector3 base_omega = getBaseOmega();
+	const btVector3 base_vel = getBaseVel();
+	const btVector3 base_omega = getBaseOmega();
 
-    // Temporary matrices/vectors -- use scratch space from caller
-    // so that we don't have to keep reallocating every frame
+	// Temporary matrices/vectors -- use scratch space from caller
+	// so that we don't have to keep reallocating every frame
 
-    scratch_r.resize(2*m_dofCount + 6);				//multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
-    scratch_v.resize(8*num_links + 6);
-    scratch_m.resize(4*num_links + 4);
+	scratch_r.resize(2 * m_dofCount + 7);  //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
+	scratch_v.resize(8 * num_links + 6);
+	scratch_m.resize(4 * num_links + 4);
 
 	//btScalar * r_ptr = &scratch_r[0];
-    btScalar * output = &scratch_r[m_dofCount];  // "output" holds the q_double_dot results
-    btVector3 * v_ptr = &scratch_v[0];
-    
-    // vhat_i  (top = angular, bottom = linear part)	
+	btScalar *output = &scratch_r[m_dofCount];  // "output" holds the q_double_dot results
+	btVector3 *v_ptr = &scratch_v[0];
+
+	// vhat_i  (top = angular, bottom = linear part)
 	btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr;
 	v_ptr += num_links * 2 + 2;
 	//
-    // zhat_i^A    
-	btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
+	// zhat_i^A
+	btSpatialForceVector *zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
 	v_ptr += num_links * 2 + 2;
 	//
-    // chat_i  (note NOT defined for the base)    
-	btSpatialMotionVector * spatCoriolisAcc = (btSpatialMotionVector *)v_ptr;
+	// chat_i  (note NOT defined for the base)
+	btSpatialMotionVector *spatCoriolisAcc = (btSpatialMotionVector *)v_ptr;
 	v_ptr += num_links * 2;
 	//
-    // Ihat_i^A.    
-	btSymmetricSpatialDyad * spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1];
+	// Ihat_i^A.
+	btSymmetricSpatialDyad *spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1];
 
-    // Cached 3x3 rotation matrices from parent frame to this frame.
-    btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
-    btMatrix3x3 * rot_from_world = &scratch_m[0];
+	// Cached 3x3 rotation matrices from parent frame to this frame.
+	btMatrix3x3 *rot_from_parent = &m_matrixBuf[0];
+	btMatrix3x3 *rot_from_world = &scratch_m[0];
 
-    // hhat_i, ahat_i
-    // hhat is NOT stored for the base (but ahat is)    
-	btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
-	btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
+	// hhat_i, ahat_i
+	// hhat is NOT stored for the base (but ahat is)
+	btSpatialForceVector *h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
+	btSpatialMotionVector *spatAcc = (btSpatialMotionVector *)v_ptr;
 	v_ptr += num_links * 2 + 2;
 	//
-    // Y_i, invD_i
-    btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
-	btScalar * Y = &scratch_r[0];
+	// Y_i, invD_i
+	btScalar *invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
+	btScalar *Y = &scratch_r[0];
 	//
-	//aux variables	
-	static btSpatialMotionVector spatJointVel;					//spatial velocity due to the joint motion (i.e. without predecessors' influence)
-	static btScalar D[36];										//"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do	
-	static btScalar invD_times_Y[6];							//D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies	
-	static btSpatialMotionVector result;							//holds results of the SolveImatrix op; it is a spatial motion vector (accel)
-	static btScalar Y_minus_hT_a[6];							//Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough	
-	static btSpatialForceVector spatForceVecTemps[6];				//6 temporary spatial force vectors
-	static btSpatialTransformationMatrix fromParent;				//spatial transform from parent to child
-	static btSymmetricSpatialDyad dyadTemp;						//inertia matrix temp
-	static btSpatialTransformationMatrix fromWorld;
+	//aux variables
+	btSpatialMotionVector spatJointVel;         //spatial velocity due to the joint motion (i.e. without predecessors' influence)
+	btScalar D[36];                             //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
+	btScalar invD_times_Y[6];                   //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
+	btSpatialMotionVector result;               //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
+	btScalar Y_minus_hT_a[6];                   //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
+	btSpatialForceVector spatForceVecTemps[6];  //6 temporary spatial force vectors
+	btSpatialTransformationMatrix fromParent;   //spatial transform from parent to child
+	btSymmetricSpatialDyad dyadTemp;            //inertia matrix temp
+	btSpatialTransformationMatrix fromWorld;
 	fromWorld.m_trnVec.setZero();
 	/////////////////
 
-    // ptr to the joint accel part of the output
-    btScalar * joint_accel = output + 6;
+	// ptr to the joint accel part of the output
+	btScalar *joint_accel = output + 6;
 
-    // Start of the algorithm proper.
-    
-    // First 'upward' loop.
-    // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
+	// Start of the algorithm proper.
 
-    rot_from_parent[0] = btMatrix3x3(m_baseQuat);				//m_baseQuat assumed to be alias!?
+	// First 'upward' loop.
+	// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
+
+	rot_from_parent[0] = btMatrix3x3(m_baseQuat);  //m_baseQuat assumed to be alias!?
 
 	//create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates
 	spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
 
-    if (m_fixedBase) 
-	{        
+	if (m_fixedBase)
+	{
 		zeroAccSpatFrc[0].setZero();
-    }
-	else 
+	}
+	else
 	{
-		btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
-		btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
-		//external forces		
-		zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));	
+		const btVector3 &baseForce = isConstraintPass ? m_baseConstraintForce : m_baseForce;
+		const btVector3 &baseTorque = isConstraintPass ? m_baseConstraintTorque : m_baseTorque;
+		//external forces
+		zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
 
 		//adding damping terms (only)
-		btScalar linDampMult = 1., angDampMult = 1.;
-		zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()),
-								   linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm()));
+		const btScalar linDampMult = 1., angDampMult = 1.;
+		zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
+									linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
 
 		//
 		//p += vhat x Ihat vhat - done in a simpler way
@@ -762,67 +820,66 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 			zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular()));
 		//
 		zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
-    }
-
+	}
 
 	//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
-	spatInertia[0].setMatrix(	btMatrix3x3(0,0,0,0,0,0,0,0,0),
-								//
-								btMatrix3x3(m_baseMass, 0, 0,
-											0, m_baseMass, 0,
-											0, 0, m_baseMass),
-								//
-								btMatrix3x3(m_baseInertia[0], 0, 0,
-											0, m_baseInertia[1], 0,
-											0, 0, m_baseInertia[2])
-							);
-
-    rot_from_world[0] = rot_from_parent[0];
+	spatInertia[0].setMatrix(btMatrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0),
+							 //
+							 btMatrix3x3(m_baseMass, 0, 0,
+										 0, m_baseMass, 0,
+										 0, 0, m_baseMass),
+							 //
+							 btMatrix3x3(m_baseInertia[0], 0, 0,
+										 0, m_baseInertia[1], 0,
+										 0, 0, m_baseInertia[2]));
+
+	rot_from_world[0] = rot_from_parent[0];
 
 	//
-    for (int i = 0; i < num_links; ++i) {		
-        const int parent = m_links[i].m_parent;
-        rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
-        rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
+	for (int i = 0; i < num_links; ++i)
+	{
+		const int parent = m_links[i].m_parent;
+		rot_from_parent[i + 1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
+		rot_from_world[i + 1] = rot_from_parent[i + 1] * rot_from_world[parent + 1];
 
-		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
-		fromWorld.m_rotMat = rot_from_world[i+1];
-		fromParent.transform(spatVel[parent+1], spatVel[i+1]);
+		fromParent.m_rotMat = rot_from_parent[i + 1];
+		fromParent.m_trnVec = m_links[i].m_cachedRVector;
+		fromWorld.m_rotMat = rot_from_world[i + 1];
+		fromParent.transform(spatVel[parent + 1], spatVel[i + 1]);
 
 		// now set vhat_i to its true value by doing
-        // vhat_i += qidot * shat_i			
-		if(!m_useGlobalVelocities)
+		// vhat_i += qidot * shat_i
+		if (!m_useGlobalVelocities)
 		{
 			spatJointVel.setZero();
 
-			for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
+			for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 				spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
 
 			// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint
-			spatVel[i+1] += spatJointVel;
+			spatVel[i + 1] += spatJointVel;
 
 			//
 			// vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint
 			//spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel;
-
 		}
 		else
 		{
-			fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i+1]);
+			fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i + 1]);
 			fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel);
 		}
 
-		// we can now calculate chat_i 		
-		spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]);		
+		// we can now calculate chat_i
+		spatVel[i + 1].cross(spatJointVel, spatCoriolisAcc[i]);
 
-        // calculate zhat_i^A
+		// calculate zhat_i^A
 		//
-		//external forces		
-		btVector3 linkAppliedForce = isConstraintPass? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
-		btVector3 linkAppliedTorque =isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
- 
-		zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce ));
-	
+		//external forces
+		btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
+		btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
+
+		zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
+
 #if 0	
 		{
 
@@ -840,27 +897,26 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 		//
 		//adding damping terms (only)
 		btScalar linDampMult = 1., angDampMult = 1.;
-		zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
-									 linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
-		
-        // calculate Ihat_i^A
+		zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()),
+										linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));
+
+		// calculate Ihat_i^A
 		//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
-		spatInertia[i+1].setMatrix(	btMatrix3x3(0,0,0,0,0,0,0,0,0),
-									//
-									btMatrix3x3(m_links[i].m_mass, 0, 0,
-												0, m_links[i].m_mass, 0,
-												0, 0, m_links[i].m_mass),
-									//
-									btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
-												0, m_links[i].m_inertiaLocal[1], 0,
-												0, 0, m_links[i].m_inertiaLocal[2])
-								);
+		spatInertia[i + 1].setMatrix(btMatrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0),
+									 //
+									 btMatrix3x3(m_links[i].m_mass, 0, 0,
+												 0, m_links[i].m_mass, 0,
+												 0, 0, m_links[i].m_mass),
+									 //
+									 btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
+												 0, m_links[i].m_inertiaLocal[1], 0,
+												 0, 0, m_links[i].m_inertiaLocal[2]));
 		//
 		//p += vhat x Ihat vhat - done in a simpler way
-		if(m_useGyroTerm)
-			zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i+1].getAngular()));			
-		//		
-		zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
+		if (m_useGyroTerm)
+			zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
+		//
+		zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
 		//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
 		////clamp parent's omega
 		//btScalar parOmegaMod = temp.length();
@@ -871,63 +927,65 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 		//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
 		//temp = spatCoriolisAcc[i].getLinear();
 		//printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
-		
-		
 
 		//printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
-		//printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());		
+		//printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());
 		//printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());
-    }
-	
-    // 'Downward' loop.
-    // (part of TreeForwardDynamics in Mirtich.)
-    for (int i = num_links - 1; i >= 0; --i)
+	}
+
+	// 'Downward' loop.
+	// (part of TreeForwardDynamics in Mirtich.)
+	for (int i = num_links - 1; i >= 0; --i)
 	{
 		const int parent = m_links[i].m_parent;
-		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
+		fromParent.m_rotMat = rot_from_parent[i + 1];
+		fromParent.m_trnVec = m_links[i].m_cachedRVector;
 
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{
 			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
 			//
-			hDof = spatInertia[i+1] * m_links[i].m_axes[dof];
+			hDof = spatInertia[i + 1] * m_links[i].m_axes[dof];
 			//
-			Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
-			- m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
-			- spatCoriolisAcc[i].dot(hDof)
-			;
+			Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]) - spatCoriolisAcc[i].dot(hDof);
 		}
-
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{
-			btScalar *D_row = &D[dof * m_links[i].m_dofCount];			
-			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+			btScalar *D_row = &D[dof * m_links[i].m_dofCount];
+			for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
 			{
-				btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
+				const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
 				D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
 			}
 		}
 
-        btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
-		switch(m_links[i].m_jointType)
+		btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
+		switch (m_links[i].m_jointType)
 		{
 			case btMultibodyLink::ePrismatic:
 			case btMultibodyLink::eRevolute:
 			{
-				invDi[0] = 1.0f / D[0];
+				if (D[0] >= SIMD_EPSILON)
+				{
+					invDi[0] = 1.0f / D[0];
+				}
+				else
+				{
+					invDi[0] = 0;
+				}
 				break;
 			}
 			case btMultibodyLink::eSpherical:
 			case btMultibodyLink::ePlanar:
 			{
-				static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
-				static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
+				const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
+				const btMatrix3x3 invD3x3(D3x3.inverse());
 
 				//unroll the loop?
-				for(int row = 0; row < 3; ++row)
+				for (int row = 0; row < 3; ++row)
 				{
-					for(int col = 0; col < 3; ++col)
-					{						
+					for (int col = 0; col < 3; ++col)
+					{
 						invDi[row * 3 + col] = invD3x3[row][col];
 					}
 				}
@@ -936,85 +994,82 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 			}
 			default:
 			{
-			
 			}
 		}
 
 		//determine h*D^{-1}
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{
 			spatForceVecTemps[dof].setZero();
 
-			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
-			{				
-				btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
-				//				
+			for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+			{
+				const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
+				//
 				spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
 			}
 		}
 
-		dyadTemp = spatInertia[i+1];
+		dyadTemp = spatInertia[i + 1];
 
 		//determine (h*D^{-1}) * h^{T}
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		{			
-			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
 			//
 			dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
 		}
 
-		fromParent.transformInverse(dyadTemp, spatInertia[parent+1], btSpatialTransformationMatrix::Add);
-        
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		fromParent.transformInverse(dyadTemp, spatInertia[parent + 1], btSpatialTransformationMatrix::Add);
+
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{
 			invD_times_Y[dof] = 0.f;
 
-			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+			for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
 			{
-				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];				
-			}	
+				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
+			}
 		}
-		
-		spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i];		
 
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
-		{				
-			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+		spatForceVecTemps[0] = zeroAccSpatFrc[i + 1] + spatInertia[i + 1] * spatCoriolisAcc[i];
+
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		{
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
 			//
-			spatForceVecTemps[0] += hDof * invD_times_Y[dof];			
+			spatForceVecTemps[0] += hDof * invD_times_Y[dof];
 		}
-		
+
 		fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
-		
-		zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
-    }
 
+		zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
+	}
 
-    // Second 'upward' loop
-    // (part of TreeForwardDynamics in Mirtich)
+	// Second 'upward' loop
+	// (part of TreeForwardDynamics in Mirtich)
 
-    if (m_fixedBase) 
+	if (m_fixedBase)
 	{
-        spatAcc[0].setZero();
-    } 
-	else 
+		spatAcc[0].setZero();
+	}
+	else
 	{
-        if (num_links > 0) 
+		if (num_links > 0)
 		{
+			m_cachedInertiaValid = true;
 			m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat;
 			m_cachedInertiaTopRight = spatInertia[0].m_topRightMat;
 			m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat;
-			m_cachedInertiaLowerRight= spatInertia[0].m_topLeftMat.transpose();
+			m_cachedInertiaLowerRight = spatInertia[0].m_topLeftMat.transpose();
+		}
 
-        }		
-		
 		solveImatrix(zeroAccSpatFrc[0], result);
 		spatAcc[0] = -result;
-    }
-	
-	
-    // now do the loop over the m_links
-    for (int i = 0; i < num_links; ++i) 
+	}
+
+	// now do the loop over the m_links
+	for (int i = 0; i < num_links; ++i)
 	{
 		//	qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar)
 		//	a = apar + cor + Sqdd
@@ -1022,79 +1077,79 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 		//	qdd = D^{-1} * (Y - h^{T}*(apar+cor))
 		//	a = apar + Sqdd
 
-        const int parent = m_links[i].m_parent;
-		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
+		const int parent = m_links[i].m_parent;
+		fromParent.m_rotMat = rot_from_parent[i + 1];
+		fromParent.m_trnVec = m_links[i].m_cachedRVector;
+
+		fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);
 
-		fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
-		
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{
-			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
-			//			
-			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);			
+			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
+			//
+			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
 		}
 
-		btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
+		btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
 		//D^{-1} * (Y - h^{T}*apar)
 		mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
 
-		spatAcc[i+1] += spatCoriolisAcc[i];		
+		spatAcc[i + 1] += spatCoriolisAcc[i];
 
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
-			spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+			spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
 
 		if (m_links[i].m_jointFeedback)
 		{
 			m_internalNeedsJointFeedback = true;
 
-			btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
-			btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
+			btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec;
+			btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec;
 
-			if (gJointFeedbackInJointFrame)
+			if (jointFeedbackInJointFrame)
 			{
 				//shift the reaction forces to the joint frame
 				//linear (force) component is the same
 				//shift the angular (torque, moment) component using the relative position,  m_links[i].m_dVector
-				 angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
+				angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
 			}
-			
 
-			if (gJointFeedbackInWorldSpace)
+			if (jointFeedbackInWorldSpace)
 			{
 				if (isConstraintPass)
 				{
- m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
-                                        m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
-				} else
+					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
+					m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
+				}
+				else
 				{
-					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
-					m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
+					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
+					m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
 				}
-			} else
+			}
+			else
 			{
 				if (isConstraintPass)
 				{
-					  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;                        
-                                m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
-
+					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
+					m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
 				}
 				else
 				{
-				m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
-				m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
-				}		
-			}	
+					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
+					m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
+				}
+			}
+		}
 	}
 
-    }
-
-    // transform base accelerations back to the world frame.
-    btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
+	// transform base accelerations back to the world frame.
+	const btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
 	output[0] = omegadot_out[0];
 	output[1] = omegadot_out[1];
 	output[2] = omegadot_out[2];
 
-    btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
+	const btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
 	output[3] = vdot_out[0];
 	output[4] = vdot_out[1];
 	output[5] = vdot_out[2];
@@ -1117,26 +1172,25 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 	//printf("]\n");
 	/////////////////
 
-    // Final step: add the accelerations (times dt) to the velocities.
+	// Final step: add the accelerations (times dt) to the velocities.
 
 	if (!isConstraintPass)
 	{
-	if(dt > 0.)
-		applyDeltaVeeMultiDof(output, dt);
-
+		if (dt > 0.)
+			applyDeltaVeeMultiDof(output, dt);
 	}
 	/////
 	//btScalar angularThres = 1;
-	//btScalar maxAngVel = 0.;		
+	//btScalar maxAngVel = 0.;
 	//bool scaleDown = 1.;
 	//for(int link = 0; link < m_links.size(); ++link)
-	//{		
+	//{
 	//	if(spatVel[link+1].getAngular().length() > maxAngVel)
 	//	{
 	//		maxAngVel = spatVel[link+1].getAngular().length();
 	//		scaleDown = angularThres / spatVel[link+1].getAngular().length();
 	//		break;
-	//	}		
+	//	}
 	//}
 
 	//if(scaleDown != 1.)
@@ -1153,128 +1207,173 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
 	/////
 
 	/////////////////////
-	if(m_useGlobalVelocities)
+	if (m_useGlobalVelocities)
 	{
-		for (int i = 0; i < num_links; ++i) 
+		for (int i = 0; i < num_links; ++i)
 		{
 			const int parent = m_links[i].m_parent;
 			//rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);    /// <- done
 			//rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];		/// <- done
-		
-			fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
-			fromWorld.m_rotMat = rot_from_world[i+1];			
-        
-			// vhat_i = i_xhat_p(i) * vhat_p(i)		
-			fromParent.transform(spatVel[parent+1], spatVel[i+1]);
+
+			fromParent.m_rotMat = rot_from_parent[i + 1];
+			fromParent.m_trnVec = m_links[i].m_cachedRVector;
+			fromWorld.m_rotMat = rot_from_world[i + 1];
+
+			// vhat_i = i_xhat_p(i) * vhat_p(i)
+			fromParent.transform(spatVel[parent + 1], spatVel[i + 1]);
 			//nice alternative below (using operator *) but it generates temps
 			/////////////////////////////////////////////////////////////
 
 			// now set vhat_i to its true value by doing
-			// vhat_i += qidot * shat_i			
+			// vhat_i += qidot * shat_i
 			spatJointVel.setZero();
 
-			for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
+			for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 				spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
-		
-			// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint
-			spatVel[i+1] += spatJointVel;
 
+			// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint
+			spatVel[i + 1] += spatJointVel;
 
-			fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity);
+			fromWorld.transformInverseRotationOnly(spatVel[i + 1], m_links[i].m_absFrameTotVelocity);
 			fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
 		}
 	}
-	
 }
 
-
-
-void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const
+void btMultiBody::solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const
 {
 	int num_links = getNumLinks();
 	///solve I * x = rhs, so the result = invI * rhs
-    if (num_links == 0) 
+	if (num_links == 0)
 	{
 		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
-        result[0] = rhs_bot[0] / m_baseInertia[0];
-        result[1] = rhs_bot[1] / m_baseInertia[1];
-        result[2] = rhs_bot[2] / m_baseInertia[2];
-        result[3] = rhs_top[0] / m_baseMass;
-        result[4] = rhs_top[1] / m_baseMass;
-        result[5] = rhs_top[2] / m_baseMass;
-    } else 
+
+		if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
+		{
+			result[0] = rhs_bot[0] / m_baseInertia[0];
+			result[1] = rhs_bot[1] / m_baseInertia[1];
+			result[2] = rhs_bot[2] / m_baseInertia[2];
+		}
+		else
+		{
+			result[0] = 0;
+			result[1] = 0;
+			result[2] = 0;
+		}
+		if (m_baseMass >= SIMD_EPSILON)
+		{
+			result[3] = rhs_top[0] / m_baseMass;
+			result[4] = rhs_top[1] / m_baseMass;
+			result[5] = rhs_top[2] / m_baseMass;
+		}
+		else
+		{
+			result[3] = 0;
+			result[4] = 0;
+			result[5] = 0;
+		}
+	}
+	else
 	{
+		if (!m_cachedInertiaValid)
+		{
+			for (int i = 0; i < 6; i++)
+			{
+				result[i] = 0.f;
+			}
+			return;
+		}
 		/// Special routine for calculating the inverse of a spatial inertia matrix
 		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
-		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
+		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse() * -1.f;
 		btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
 		btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
 		tmp = invIupper_right * m_cachedInertiaLowerRight;
 		btMatrix3x3 invI_upper_left = (tmp * Binv);
 		btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
-		tmp = m_cachedInertiaTopLeft  * invI_upper_left;
-		tmp[0][0]-= 1.0;
-		tmp[1][1]-= 1.0;
-		tmp[2][2]-= 1.0;
+		tmp = m_cachedInertiaTopLeft * invI_upper_left;
+		tmp[0][0] -= 1.0;
+		tmp[1][1] -= 1.0;
+		tmp[2][2] -= 1.0;
 		btMatrix3x3 invI_lower_left = (Binv * tmp);
 
 		//multiply result = invI * rhs
 		{
-		  btVector3 vtop = invI_upper_left*rhs_top;
-		  btVector3 tmp;
-		  tmp = invIupper_right * rhs_bot;
-		  vtop += tmp;
-		  btVector3 vbot = invI_lower_left*rhs_top;
-		  tmp = invI_lower_right * rhs_bot;
-		  vbot += tmp;
-		  result[0] = vtop[0];
-		  result[1] = vtop[1];
-		  result[2] = vtop[2];
-		  result[3] = vbot[0];
-		  result[4] = vbot[1];
-		  result[5] = vbot[2];
+			btVector3 vtop = invI_upper_left * rhs_top;
+			btVector3 tmp;
+			tmp = invIupper_right * rhs_bot;
+			vtop += tmp;
+			btVector3 vbot = invI_lower_left * rhs_top;
+			tmp = invI_lower_right * rhs_bot;
+			vbot += tmp;
+			result[0] = vtop[0];
+			result[1] = vtop[1];
+			result[2] = vtop[2];
+			result[3] = vbot[0];
+			result[4] = vbot[1];
+			result[5] = vbot[2];
 		}
-		
-    }
+	}
 }
 void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const
 {
 	int num_links = getNumLinks();
 	///solve I * x = rhs, so the result = invI * rhs
-    if (num_links == 0) 
+	if (num_links == 0)
 	{
 		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
-		result.setAngular(rhs.getAngular() / m_baseInertia);
-		result.setLinear(rhs.getLinear() / m_baseMass);		
-    } else 
+		if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON))
+		{
+			result.setAngular(rhs.getAngular() / m_baseInertia);
+		}
+		else
+		{
+			result.setAngular(btVector3(0, 0, 0));
+		}
+		if (m_baseMass >= SIMD_EPSILON)
+		{
+			result.setLinear(rhs.getLinear() / m_baseMass);
+		}
+		else
+		{
+			result.setLinear(btVector3(0, 0, 0));
+		}
+	}
+	else
 	{
 		/// Special routine for calculating the inverse of a spatial inertia matrix
 		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
-		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
+		if (!m_cachedInertiaValid)
+		{
+			result.setLinear(btVector3(0, 0, 0));
+			result.setAngular(btVector3(0, 0, 0));
+			result.setVector(btVector3(0, 0, 0), btVector3(0, 0, 0));
+			return;
+		}
+		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse() * -1.f;
 		btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
 		btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
 		tmp = invIupper_right * m_cachedInertiaLowerRight;
 		btMatrix3x3 invI_upper_left = (tmp * Binv);
 		btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
-		tmp = m_cachedInertiaTopLeft  * invI_upper_left;
-		tmp[0][0]-= 1.0;
-		tmp[1][1]-= 1.0;
-		tmp[2][2]-= 1.0;
+		tmp = m_cachedInertiaTopLeft * invI_upper_left;
+		tmp[0][0] -= 1.0;
+		tmp[1][1] -= 1.0;
+		tmp[2][2] -= 1.0;
 		btMatrix3x3 invI_lower_left = (Binv * tmp);
 
 		//multiply result = invI * rhs
 		{
-		  btVector3 vtop = invI_upper_left*rhs.getLinear();
-		  btVector3 tmp;
-		  tmp = invIupper_right * rhs.getAngular();
-		  vtop += tmp;
-		  btVector3 vbot = invI_lower_left*rhs.getLinear();
-		  tmp = invI_lower_right * rhs.getAngular();
-		  vbot += tmp;
-		  result.setVector(vtop, vbot);		  
+			btVector3 vtop = invI_upper_left * rhs.getLinear();
+			btVector3 tmp;
+			tmp = invIupper_right * rhs.getAngular();
+			vtop += tmp;
+			btVector3 vbot = invI_lower_left * rhs.getLinear();
+			tmp = invI_lower_right * rhs.getAngular();
+			vbot += tmp;
+			result.setVector(vtop, vbot);
 		}
-		
-    }
+	}
 }
 
 void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
@@ -1293,155 +1392,152 @@ void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, in
 }
 
 void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
-                                       btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
+												 btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
 {
-    // Temporary matrices/vectors -- use scratch space from caller
-    // so that we don't have to keep reallocating every frame
+	// Temporary matrices/vectors -- use scratch space from caller
+	// so that we don't have to keep reallocating every frame
 
-	
-	int num_links = getNumLinks();	
-    scratch_r.resize(m_dofCount);
-    scratch_v.resize(4*num_links + 4);	    
+	int num_links = getNumLinks();
+	scratch_r.resize(m_dofCount);
+	scratch_v.resize(4 * num_links + 4);
 
-    btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0;
-    btVector3 * v_ptr = &scratch_v[0];
+	btScalar *r_ptr = m_dofCount ? &scratch_r[0] : 0;
+	btVector3 *v_ptr = &scratch_v[0];
 
-    // zhat_i^A (scratch space)
-    btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
+	// zhat_i^A (scratch space)
+	btSpatialForceVector *zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
 	v_ptr += num_links * 2 + 2;
 
-    // rot_from_parent (cached from calcAccelerations)
-    const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
+	// rot_from_parent (cached from calcAccelerations)
+	const btMatrix3x3 *rot_from_parent = &m_matrixBuf[0];
 
-    // hhat (cached), accel (scratch)
-    // hhat is NOT stored for the base (but ahat is) 
-	const btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
-	btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
+	// hhat (cached), accel (scratch)
+	// hhat is NOT stored for the base (but ahat is)
+	const btSpatialForceVector *h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
+	btSpatialMotionVector *spatAcc = (btSpatialMotionVector *)v_ptr;
 	v_ptr += num_links * 2 + 2;
 
-    // Y_i (scratch), invD_i (cached)
-    const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
-	btScalar * Y = r_ptr; 
+	// Y_i (scratch), invD_i (cached)
+	const btScalar *invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
+	btScalar *Y = r_ptr;
 	////////////////
 	//aux variables
-	static btScalar invD_times_Y[6];							//D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
-	static btSpatialMotionVector result;							//holds results of the SolveImatrix op; it is a spatial motion vector (accel)
-	static btScalar Y_minus_hT_a[6];							//Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough	
-	static btSpatialForceVector spatForceVecTemps[6];				//6 temporary spatial force vectors
-	static btSpatialTransformationMatrix fromParent;	
+	btScalar invD_times_Y[6];                   //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
+	btSpatialMotionVector result;               //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
+	btScalar Y_minus_hT_a[6];                   //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
+	btSpatialForceVector spatForceVecTemps[6];  //6 temporary spatial force vectors
+	btSpatialTransformationMatrix fromParent;
 	/////////////////
 
-    // First 'upward' loop.
-    // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
-	
+	// First 'upward' loop.
+	// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
+
 	// Fill in zero_acc
-    // -- set to force/torque on the base, zero otherwise
-    if (m_fixedBase) 
+	// -- set to force/torque on the base, zero otherwise
+	if (m_fixedBase)
+	{
+		zeroAccSpatFrc[0].setZero();
+	}
+	else
 	{
-        zeroAccSpatFrc[0].setZero();
-    } else 
-	{	
 		//test forces
 		fromParent.m_rotMat = rot_from_parent[0];
-		fromParent.transformRotationOnly(btSpatialForceVector(-force[0],-force[1],-force[2], -force[3],-force[4],-force[5]), zeroAccSpatFrc[0]);
-    }
-    for (int i = 0; i < num_links; ++i) 
+		fromParent.transformRotationOnly(btSpatialForceVector(-force[0], -force[1], -force[2], -force[3], -force[4], -force[5]), zeroAccSpatFrc[0]);
+	}
+	for (int i = 0; i < num_links; ++i)
 	{
-		zeroAccSpatFrc[i+1].setZero();
-    }    
+		zeroAccSpatFrc[i + 1].setZero();
+	}
 
 	// 'Downward' loop.
-    // (part of TreeForwardDynamics in Mirtich.)
-    for (int i = num_links - 1; i >= 0; --i)
+	// (part of TreeForwardDynamics in Mirtich.)
+	for (int i = num_links - 1; i >= 0; --i)
 	{
 		const int parent = m_links[i].m_parent;
-		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
+		fromParent.m_rotMat = rot_from_parent[i + 1];
+		fromParent.m_trnVec = m_links[i].m_cachedRVector;
 
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{
-			Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
-											- m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
-											;
+			Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]);
 		}
 
 		btVector3 in_top, in_bottom, out_top, out_bottom;
-		const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
-		
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		const btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
+
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{
 			invD_times_Y[dof] = 0.f;
 
-			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
+			for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
 			{
-				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];				
-			}	
+				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
+			}
 		}
-		
-		 // Zp += pXi * (Zi + hi*Yi/Di)
-		spatForceVecTemps[0] = zeroAccSpatFrc[i+1];
 
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		// Zp += pXi * (Zi + hi*Yi/Di)
+		spatForceVecTemps[0] = zeroAccSpatFrc[i + 1];
+
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{
 			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
 			//
-			spatForceVecTemps[0] += hDof * invD_times_Y[dof];		
+			spatForceVecTemps[0] += hDof * invD_times_Y[dof];
 		}
-		
 
 		fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
-			
-		zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
-    }
-	
-	// ptr to the joint accel part of the output
-    btScalar * joint_accel = output + 6;
 
+		zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
+	}
+
+	// ptr to the joint accel part of the output
+	btScalar *joint_accel = output + 6;
 
-    // Second 'upward' loop
-    // (part of TreeForwardDynamics in Mirtich)
+	// Second 'upward' loop
+	// (part of TreeForwardDynamics in Mirtich)
 
-    if (m_fixedBase) 
+	if (m_fixedBase)
 	{
-        spatAcc[0].setZero();
-    } 
-	else 
+		spatAcc[0].setZero();
+	}
+	else
 	{
 		solveImatrix(zeroAccSpatFrc[0], result);
 		spatAcc[0] = -result;
+	}
 
-    }
-	
-    // now do the loop over the m_links
-    for (int i = 0; i < num_links; ++i)
+	// now do the loop over the m_links
+	for (int i = 0; i < num_links; ++i)
 	{
-        const int parent = m_links[i].m_parent;
-		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
+		const int parent = m_links[i].m_parent;
+		fromParent.m_rotMat = rot_from_parent[i + 1];
+		fromParent.m_trnVec = m_links[i].m_cachedRVector;
 
-		fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
-		
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+		fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);
+
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
 		{
 			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
-			//			
-			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);
+			//
+			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
 		}
 
-		const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
-		mulMatrix(const_cast<btScalar*>(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
+		const btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
+		mulMatrix(const_cast<btScalar *>(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
 
-		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
-			spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];      
-    }
+		for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
+			spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
+	}
 
-    // transform base accelerations back to the world frame.
-    btVector3 omegadot_out;
-    omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
+	// transform base accelerations back to the world frame.
+	btVector3 omegadot_out;
+	omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
 	output[0] = omegadot_out[0];
 	output[1] = omegadot_out[1];
 	output[2] = omegadot_out[2];
 
-    btVector3 vdot_out;
-    vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear();
+	btVector3 vdot_out;
+	vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear();
 	output[3] = vdot_out[0];
 	output[4] = vdot_out[1];
 	output[5] = vdot_out[2];
@@ -1453,20 +1549,169 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 	//printf("]\n");
 	/////////////////
 }
+void btMultiBody::predictPositionsMultiDof(btScalar dt)
+{
+    int num_links = getNumLinks();
+    // step position by adding dt * velocity
+    //btVector3 v = getBaseVel();
+    //m_basePos += dt * v;
+    //
+    btScalar *pBasePos;
+    btScalar *pBaseVel = &m_realBuf[3];  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
+    
+    // reset to current position
+    for (int i = 0; i < 3; ++i)
+    {
+        m_basePos_interpolate[i] = m_basePos[i];
+    }
+    pBasePos = m_basePos_interpolate;
+    
+    pBasePos[0] += dt * pBaseVel[0];
+    pBasePos[1] += dt * pBaseVel[1];
+    pBasePos[2] += dt * pBaseVel[2];
+    
+    ///////////////////////////////
+    //local functor for quaternion integration (to avoid error prone redundancy)
+    struct
+    {
+        //"exponential map" based on btTransformUtil::integrateTransform(..)
+        void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
+        {
+            //baseBody    =>    quat is alias and omega is global coor
+            //!baseBody    =>    quat is alibi and omega is local coor
+            
+            btVector3 axis;
+            btVector3 angvel;
+            
+            if (!baseBody)
+                angvel = quatRotate(quat, omega);  //if quat is not m_baseQuat, it is alibi => ok
+            else
+                angvel = omega;
+            
+            btScalar fAngle = angvel.length();
+            //limit the angular motion
+            if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
+            {
+                fAngle = btScalar(0.5) * SIMD_HALF_PI / dt;
+            }
+            
+            if (fAngle < btScalar(0.001))
+            {
+                // use Taylor's expansions of sync function
+                axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle);
+            }
+            else
+            {
+                // sync(fAngle) = sin(c*fAngle)/t
+                axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle);
+            }
+            
+            if (!baseBody)
+                quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat;
+            else
+                quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5)));
+            //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();
+            
+            quat.normalize();
+        }
+    } pQuatUpdateFun;
+    ///////////////////////////////
+    
+    //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
+    //
+    btScalar *pBaseQuat;
 
+    // reset to current orientation
+    for (int i = 0; i < 4; ++i)
+    {
+        m_baseQuat_interpolate[i] = m_baseQuat[i];
+    }
+    pBaseQuat = m_baseQuat_interpolate;
 
-
+    btScalar *pBaseOmega = &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
+    //
+    btQuaternion baseQuat;
+    baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
+    btVector3 baseOmega;
+    baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
+    pQuatUpdateFun(baseOmega, baseQuat, true, dt);
+    pBaseQuat[0] = baseQuat.x();
+    pBaseQuat[1] = baseQuat.y();
+    pBaseQuat[2] = baseQuat.z();
+    pBaseQuat[3] = baseQuat.w();
+
+    // Finally we can update m_jointPos for each of the m_links
+    for (int i = 0; i < num_links; ++i)
+    {
+        btScalar *pJointPos;
+        pJointPos = &m_links[i].m_jointPos_interpolate[0];
+        
+        btScalar *pJointVel = getJointVelMultiDof(i);
+        
+        switch (m_links[i].m_jointType)
+        {
+            case btMultibodyLink::ePrismatic:
+            case btMultibodyLink::eRevolute:
+            {
+                //reset to current pos
+                pJointPos[0] = m_links[i].m_jointPos[0];
+                btScalar jointVel = pJointVel[0];
+                pJointPos[0] += dt * jointVel;
+                break;
+            }
+            case btMultibodyLink::eSpherical:
+            {
+                //reset to current pos
+
+                for (int j = 0; j < 4; ++j)
+                {
+                    pJointPos[j] = m_links[i].m_jointPos[j];
+                }
+                
+                btVector3 jointVel;
+                jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
+                btQuaternion jointOri;
+                jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
+                pQuatUpdateFun(jointVel, jointOri, false, dt);
+                pJointPos[0] = jointOri.x();
+                pJointPos[1] = jointOri.y();
+                pJointPos[2] = jointOri.z();
+                pJointPos[3] = jointOri.w();
+                break;
+            }
+            case btMultibodyLink::ePlanar:
+            {
+                for (int j = 0; j < 3; ++j)
+                {
+                    pJointPos[j] = m_links[i].m_jointPos[j];
+                }
+                pJointPos[0] += dt * getJointVelMultiDof(i)[0];
+                
+                btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
+                btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
+                pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
+                pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
+                break;
+            }
+            default:
+            {
+            }
+        }
+        
+        m_links[i].updateInterpolationCacheMultiDof();
+    }
+}
 
 void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
-{	
+{
 	int num_links = getNumLinks();
-    // step position by adding dt * velocity
-	//btVector3 v = getBaseVel();	
-    //m_basePos += dt * v;
+	// step position by adding dt * velocity
+	//btVector3 v = getBaseVel();
+	//m_basePos += dt * v;
 	//
-	btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
-	btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);			//note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
-	//	
+    btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
+    btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
+    
 	pBasePos[0] += dt * pBaseVel[0];
 	pBasePos[1] += dt * pBaseVel[1];
 	pBasePos[2] += dt * pBaseVel[2];
@@ -1476,92 +1721,102 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 	struct
 	{
 		//"exponential map" based on btTransformUtil::integrateTransform(..)
-		void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
+		void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
 		{
 			//baseBody	=>	quat is alias and omega is global coor
-			//!baseBody	=>	quat is alibi and omega is local coor	
-			
+			//!baseBody	=>	quat is alibi and omega is local coor
+
 			btVector3 axis;
 			btVector3 angvel;
 
-			if(!baseBody)			
-				angvel = quatRotate(quat, omega);				//if quat is not m_baseQuat, it is alibi => ok			
+			if (!baseBody)
+				angvel = quatRotate(quat, omega);  //if quat is not m_baseQuat, it is alibi => ok
 			else
 				angvel = omega;
-		
-			btScalar fAngle = angvel.length(); 		
+
+			btScalar fAngle = angvel.length();
 			//limit the angular motion
 			if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
 			{
-				fAngle = btScalar(0.5)*SIMD_HALF_PI / dt;
+				fAngle = btScalar(0.5) * SIMD_HALF_PI / dt;
 			}
 
-			if ( fAngle < btScalar(0.001) )
+			if (fAngle < btScalar(0.001))
 			{
 				// use Taylor's expansions of sync function
-				axis   = angvel*( btScalar(0.5)*dt-(dt*dt*dt)*(btScalar(0.020833333333))*fAngle*fAngle );
+				axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle);
 			}
 			else
 			{
 				// sync(fAngle) = sin(c*fAngle)/t
-				axis   = angvel*( btSin(btScalar(0.5)*fAngle*dt)/fAngle );
+				axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle);
 			}
-			
-			if(!baseBody)				
-				quat = btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat;			
-			else			
-				quat = quat * btQuaternion(-axis.x(),-axis.y(),-axis.z(),btCos( fAngle*dt*btScalar(0.5) ));
-				//equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();			
-		
+
+			if (!baseBody)
+				quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat;
+			else
+				quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5)));
+			//equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();
+
 			quat.normalize();
 		}
 	} pQuatUpdateFun;
 	///////////////////////////////
 
 	//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
-	//	
-	btScalar *pBaseQuat = pq ? pq : m_baseQuat;	
-	btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];		//note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
 	//
-	static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
-	static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
+    btScalar *pBaseQuat = pq ? pq : m_baseQuat;
+	btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
+	//
+	btQuaternion baseQuat;
+	baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
+	btVector3 baseOmega;
+	baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
 	pQuatUpdateFun(baseOmega, baseQuat, true, dt);
 	pBaseQuat[0] = baseQuat.x();
 	pBaseQuat[1] = baseQuat.y();
 	pBaseQuat[2] = baseQuat.z();
 	pBaseQuat[3] = baseQuat.w();
 
-
 	//printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
 	//printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
 	//printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
 
-	if(pq)		
+	if (pq)
 		pq += 7;
-	if(pqd)
+	if (pqd)
 		pqd += 6;
 
 	// Finally we can update m_jointPos for each of the m_links
-    for (int i = 0; i < num_links; ++i) 
+	for (int i = 0; i < num_links; ++i)
 	{
-		btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);		
+        btScalar *pJointPos;
+        pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
+        
 		btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
 
-		switch(m_links[i].m_jointType)
+		switch (m_links[i].m_jointType)
 		{
 			case btMultibodyLink::ePrismatic:
 			case btMultibodyLink::eRevolute:
 			{
-				btScalar jointVel = pJointVel[0];	
+                //reset to current pos
+				btScalar jointVel = pJointVel[0];
 				pJointPos[0] += dt * jointVel;
 				break;
 			}
 			case btMultibodyLink::eSpherical:
 			{
-				static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
-				static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
+                //reset to current pos
+				btVector3 jointVel;
+				jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
+				btQuaternion jointOri;
+				jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
 				pQuatUpdateFun(jointVel, jointOri, false, dt);
-				pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
+				pJointPos[0] = jointOri.x();
+				pJointPos[1] = jointOri.y();
+				pJointPos[2] = jointOri.z();
+				pJointPos[3] = jointOri.w();
 				break;
 			}
 			case btMultibodyLink::ePlanar:
@@ -1578,122 +1833,135 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 			default:
 			{
 			}
-
 		}
 
 		m_links[i].updateCacheMultiDof(pq);
 
-		if(pq)		
+		if (pq)
 			pq += m_links[i].m_posVarCount;
-		if(pqd)
+		if (pqd)
 			pqd += m_links[i].m_dofCount;
-    }
+	}
 }
 
 void btMultiBody::fillConstraintJacobianMultiDof(int link,
-                                    const btVector3 &contact_point,
-									const btVector3 &normal_ang,
-                                    const btVector3 &normal_lin,
-                                    btScalar *jac,
-                                    btAlignedObjectArray<btScalar> &scratch_r,
-                                    btAlignedObjectArray<btVector3> &scratch_v,
-                                    btAlignedObjectArray<btMatrix3x3> &scratch_m) const
-{
-    // temporary space
+												 const btVector3 &contact_point,
+												 const btVector3 &normal_ang,
+												 const btVector3 &normal_lin,
+												 btScalar *jac,
+												 btAlignedObjectArray<btScalar> &scratch_r1,
+												 btAlignedObjectArray<btVector3> &scratch_v,
+												 btAlignedObjectArray<btMatrix3x3> &scratch_m) const
+{
+	// temporary space
 	int num_links = getNumLinks();
 	int m_dofCount = getNumDofs();
-    scratch_v.resize(3*num_links + 3);			//(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang
-    scratch_m.resize(num_links + 1);
-
-    btVector3 * v_ptr = &scratch_v[0];
-    btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1;
-    btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1;
-	btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1;
-    btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
-
-    scratch_r.resize(m_dofCount);
-    btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0;
-
-    btMatrix3x3 * rot_from_world = &scratch_m[0];
+	scratch_v.resize(3 * num_links + 3);  //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang
+	scratch_m.resize(num_links + 1);
+
+	btVector3 *v_ptr = &scratch_v[0];
+	btVector3 *p_minus_com_local = v_ptr;
+	v_ptr += num_links + 1;
+	btVector3 *n_local_lin = v_ptr;
+	v_ptr += num_links + 1;
+	btVector3 *n_local_ang = v_ptr;
+	v_ptr += num_links + 1;
+	btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
+
+	//scratch_r.resize(m_dofCount);
+	//btScalar *results = m_dofCount > 0 ? &scratch_r[0] : 0;
+
+    scratch_r1.resize(m_dofCount+num_links);
+    btScalar * results = m_dofCount > 0 ? &scratch_r1[0] : 0;
+    btScalar* links = num_links? &scratch_r1[m_dofCount] : 0;
+    int numLinksChildToRoot=0;
+    int l = link;
+    while (l != -1)
+    {
+        links[numLinksChildToRoot++]=l;
+        l = m_links[l].m_parent;
+    }
+    
+	btMatrix3x3 *rot_from_world = &scratch_m[0];
 
-    const btVector3 p_minus_com_world = contact_point - m_basePos;
-	const btVector3 &normal_lin_world = normal_lin;							//convenience
+	const btVector3 p_minus_com_world = contact_point - m_basePos;
+	const btVector3 &normal_lin_world = normal_lin;  //convenience
 	const btVector3 &normal_ang_world = normal_ang;
 
-    rot_from_world[0] = btMatrix3x3(m_baseQuat);    
-    
-    // omega coeffients first.
-    btVector3 omega_coeffs_world;
-    omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);
+	rot_from_world[0] = btMatrix3x3(m_baseQuat);
+
+	// omega coeffients first.
+	btVector3 omega_coeffs_world;
+	omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);
 	jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
 	jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
 	jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
-    // then v coefficients
-    jac[3] = normal_lin_world[0];
-    jac[4] = normal_lin_world[1];
-    jac[5] = normal_lin_world[2];
+	// then v coefficients
+	jac[3] = normal_lin_world[0];
+	jac[4] = normal_lin_world[1];
+	jac[5] = normal_lin_world[2];
 
 	//create link-local versions of p_minus_com and normal
 	p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
-    n_local_lin[0] = rot_from_world[0] * normal_lin_world;
+	n_local_lin[0] = rot_from_world[0] * normal_lin_world;
 	n_local_ang[0] = rot_from_world[0] * normal_ang_world;
 
-    // Set remaining jac values to zero for now.
-    for (int i = 6; i < 6 + m_dofCount; ++i) 
+	// Set remaining jac values to zero for now.
+	for (int i = 6; i < 6 + m_dofCount; ++i)
 	{
-        jac[i] = 0;
-    }
-
-    // Qdot coefficients, if necessary.
-    if (num_links > 0 && link > -1) {
-
-        // TODO: speed this up -- don't calculate for m_links we don't need.
-        // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
-        // which is resulting in repeated work being done...)
-
-        // calculate required normals & positions in the local frames.
-        for (int i = 0; i < num_links; ++i) {
+		jac[i] = 0;
+	}
 
-            // transform to local frame
-            const int parent = m_links[i].m_parent;
-            const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
-            rot_from_world[i+1] = mtx * rot_from_world[parent+1];
+	// Qdot coefficients, if necessary.
+	if (num_links > 0 && link > -1)
+	{
+        // TODO: (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
+		// which is resulting in repeated work being done...)
+
+		// calculate required normals & positions in the local frames.
+        for (int a = 0; a < numLinksChildToRoot; a++)
+        {
+            int i = links[numLinksChildToRoot-1-a];
+        	// transform to local frame
+			const int parent = m_links[i].m_parent;
+			const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
+			rot_from_world[i + 1] = mtx * rot_from_world[parent + 1];
 
-            n_local_lin[i+1] = mtx * n_local_lin[parent+1];
-			n_local_ang[i+1] = mtx * n_local_ang[parent+1];
-            p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - m_links[i].m_cachedRVector;
+			n_local_lin[i + 1] = mtx * n_local_lin[parent + 1];
+			n_local_ang[i + 1] = mtx * n_local_ang[parent + 1];
+			p_minus_com_local[i + 1] = mtx * p_minus_com_local[parent + 1] - m_links[i].m_cachedRVector;
 
 			// calculate the jacobian entry
-			switch(m_links[i].m_jointType)
+			switch (m_links[i].m_jointType)
 			{
 				case btMultibodyLink::eRevolute:
 				{
-					results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
-					results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
+					results[m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(0));
+					results[m_links[i].m_dofOffset] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0));
 					break;
 				}
 				case btMultibodyLink::ePrismatic:
 				{
-					results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0));
+					results[m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(0));
 					break;
 				}
 				case btMultibodyLink::eSpherical:
 				{
-					results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
-					results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(1));
-					results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(2));
-										
-					results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
-					results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1));
-					results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2));
+					results[m_links[i].m_dofOffset + 0] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(0));
+					results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(1));
+					results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(2));
+
+					results[m_links[i].m_dofOffset + 0] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0));
+					results[m_links[i].m_dofOffset + 1] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(1));
+					results[m_links[i].m_dofOffset + 2] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(2));
 
 					break;
 				}
 				case btMultibodyLink::ePlanar:
 				{
-					results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
-					results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1));
-					results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2));
+					results[m_links[i].m_dofOffset + 0] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]));  // + m_links[i].getAxisBottom(0));
+					results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(1));
+					results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(2));
 
 					break;
 				}
@@ -1701,246 +1969,318 @@ void btMultiBody::fillConstraintJacobianMultiDof(int link,
 				{
 				}
 			}
-            
-        }
+		}
 
-        // Now copy through to output.
+		// Now copy through to output.
 		//printf("jac[%d] = ", link);
-        while (link != -1) 
+		while (link != -1)
 		{
-			for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
+			for (int dof = 0; dof < m_links[link].m_dofCount; ++dof)
 			{
 				jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
 				//printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);
 			}
-            
+
 			link = m_links[link].m_parent;
-        }
+		}
 		//printf("]\n");
-    }
+	}
 }
 
-
 void btMultiBody::wakeUp()
 {
-    m_awake = true;
+	m_sleepTimer = 0;
+	m_awake = true;
 }
 
 void btMultiBody::goToSleep()
 {
-    m_awake = false;
+	m_awake = false;
 }
 
 void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
 {
 	extern bool gDisableDeactivation;
-    if (!m_canSleep || gDisableDeactivation) 
+	if (!m_canSleep || gDisableDeactivation)
 	{
 		m_awake = true;
 		m_sleepTimer = 0;
 		return;
 	}
 
-    // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
-    btScalar motion = 0;
+	
+
+	// motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
+	btScalar motion = 0;
 	{
-		for (int i = 0; i < 6 + m_dofCount; ++i) 		
+		for (int i = 0; i < 6 + m_dofCount; ++i)
 			motion += m_realBuf[i] * m_realBuf[i];
 	}
-	
 
-    if (motion < SLEEP_EPSILON) {
-        m_sleepTimer += timestep;
-        if (m_sleepTimer > SLEEP_TIMEOUT) {
-            goToSleep();
-        }
-    } else {
-        m_sleepTimer = 0;
-		if (!m_awake)
-			wakeUp();
-    }
+	if (motion < SLEEP_EPSILON)
+	{
+		m_sleepTimer += timestep;
+		if (m_sleepTimer > SLEEP_TIMEOUT)
+		{
+			goToSleep();
+		}
+	}
+	else
+	{
+		m_sleepTimer = 0;
+		if (m_canWakeup)
+		{
+			if (!m_awake)
+				wakeUp();
+		}
+	}
 }
 
-
-void	btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin)
+void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
 {
-	
 	int num_links = getNumLinks();
 
 	// Cached 3x3 rotation matrices from parent frame to this frame.
-	btMatrix3x3* rot_from_parent =(btMatrix3x3 *) &m_matrixBuf[0];
+	btMatrix3x3 *rot_from_parent = (btMatrix3x3 *)&m_matrixBuf[0];
 
-	rot_from_parent[0] = btMatrix3x3(m_baseQuat);				//m_baseQuat assumed to be alias!?
-	
-	for (int i = 0; i < num_links; ++i) 
+	rot_from_parent[0] = btMatrix3x3(m_baseQuat);  //m_baseQuat assumed to be alias!?
+
+	for (int i = 0; i < num_links; ++i)
 	{
-		rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
+		rot_from_parent[i + 1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
 	}
-		
+
 	int nLinks = getNumLinks();
 	///base + num m_links
-	world_to_local.resize(nLinks+1);
-	local_origin.resize(nLinks+1);
+	world_to_local.resize(nLinks + 1);
+	local_origin.resize(nLinks + 1);
 
 	world_to_local[0] = getWorldToBaseRot();
 	local_origin[0] = getBasePos();
-	
-	for (int k=0;k<getNumLinks();k++)
+
+	for (int k = 0; k < getNumLinks(); k++)
 	{
 		const int parent = getParent(k);
-		world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
-		local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
+		world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1];
+		local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k)));
 	}
 
-	for (int link=0;link<getNumLinks();link++)
+	for (int link = 0; link < getNumLinks(); link++)
 	{
-		int index = link+1;
+		int index = link + 1;
 
 		btVector3 posr = local_origin[index];
-		btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
+		btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
 		btTransform tr;
 		tr.setIdentity();
 		tr.setOrigin(posr);
-		tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
+		tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
 		getLink(link).m_cachedWorldTransform = tr;
-			
 	}
-
 }
 
-void	btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin)
+void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
 {
-	world_to_local.resize(getNumLinks()+1);
-	local_origin.resize(getNumLinks()+1);
-	
+	world_to_local.resize(getNumLinks() + 1);
+	local_origin.resize(getNumLinks() + 1);
+
 	world_to_local[0] = getWorldToBaseRot();
 	local_origin[0] = getBasePos();
-	
+
 	if (getBaseCollider())
 	{
 		btVector3 posr = local_origin[0];
 		//	float pos[4]={posr.x(),posr.y(),posr.z(),1};
-		btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
+		btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
 		btTransform tr;
 		tr.setIdentity();
 		tr.setOrigin(posr);
-		tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
-		
+		tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+
 		getBaseCollider()->setWorldTransform(tr);
-		
+        getBaseCollider()->setInterpolationWorldTransform(tr);
 	}
-	
-	for (int k=0;k<getNumLinks();k++)
+
+	for (int k = 0; k < getNumLinks(); k++)
 	{
 		const int parent = getParent(k);
-		world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
-		local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
+		world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1];
+		local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k)));
 	}
-	
-	
-	for (int m=0;m<getNumLinks();m++)
+
+	for (int m = 0; m < getNumLinks(); m++)
 	{
-		btMultiBodyLinkCollider* col = getLink(m).m_collider;
+		btMultiBodyLinkCollider *col = getLink(m).m_collider;
 		if (col)
 		{
 			int link = col->m_link;
 			btAssert(link == m);
-			
-			int index = link+1;
-			
+
+			int index = link + 1;
+
 			btVector3 posr = local_origin[index];
 			//			float pos[4]={posr.x(),posr.y(),posr.z(),1};
-			btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
+			btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
 			btTransform tr;
 			tr.setIdentity();
 			tr.setOrigin(posr);
-			tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
-			
+			tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+
 			col->setWorldTransform(tr);
+            col->setInterpolationWorldTransform(tr);
 		}
 	}
 }
 
-int	btMultiBody::calculateSerializeBufferSize()	const
+void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
+{
+    world_to_local.resize(getNumLinks() + 1);
+    local_origin.resize(getNumLinks() + 1);
+    
+    world_to_local[0] = getInterpolateWorldToBaseRot();
+    local_origin[0] = getInterpolateBasePos();
+    
+    if (getBaseCollider())
+    {
+        btVector3 posr = local_origin[0];
+        //    float pos[4]={posr.x(),posr.y(),posr.z(),1};
+        btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
+        btTransform tr;
+        tr.setIdentity();
+        tr.setOrigin(posr);
+        tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+        
+        getBaseCollider()->setInterpolationWorldTransform(tr);
+    }
+    
+    for (int k = 0; k < getNumLinks(); k++)
+    {
+        const int parent = getParent(k);
+        world_to_local[k + 1] = getInterpolateParentToLocalRot(k) * world_to_local[parent + 1];
+        local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getInterpolateRVector(k)));
+    }
+    
+    for (int m = 0; m < getNumLinks(); m++)
+    {
+        btMultiBodyLinkCollider *col = getLink(m).m_collider;
+        if (col)
+        {
+            int link = col->m_link;
+            btAssert(link == m);
+            
+            int index = link + 1;
+            
+            btVector3 posr = local_origin[index];
+            //            float pos[4]={posr.x(),posr.y(),posr.z(),1};
+            btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
+            btTransform tr;
+            tr.setIdentity();
+            tr.setOrigin(posr);
+            tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+            
+            col->setInterpolationWorldTransform(tr);
+        }
+    }
+}
+
+int btMultiBody::calculateSerializeBufferSize() const
 {
 	int sz = sizeof(btMultiBodyData);
 	return sz;
 }
 
-	///fills the dataBuffer and returns the struct name (and 0 on failure)
-const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char *btMultiBody::serialize(void *dataBuffer, class btSerializer *serializer) const
 {
-		btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer;
-		getBaseWorldTransform().serialize(mbd->m_baseWorldTransform);
-		mbd->m_baseMass = this->getBaseMass();
-		getBaseInertia().serialize(mbd->m_baseInertia);
+	btMultiBodyData *mbd = (btMultiBodyData *)dataBuffer;
+	getBasePos().serialize(mbd->m_baseWorldPosition);
+	getWorldToBaseRot().inverse().serialize(mbd->m_baseWorldOrientation);
+	getBaseVel().serialize(mbd->m_baseLinearVelocity);
+	getBaseOmega().serialize(mbd->m_baseAngularVelocity);
+
+	mbd->m_baseMass = this->getBaseMass();
+	getBaseInertia().serialize(mbd->m_baseInertia);
+	{
+		char *name = (char *)serializer->findNameForPointer(m_baseName);
+		mbd->m_baseName = (char *)serializer->getUniquePointer(name);
+		if (mbd->m_baseName)
 		{
-			char* name = (char*) serializer->findNameForPointer(m_baseName);
-			mbd->m_baseName = (char*)serializer->getUniquePointer(name);
-			if (mbd->m_baseName)
-			{
-				serializer->serializeName(name);
-			}
+			serializer->serializeName(name);
 		}
-		mbd->m_numLinks = this->getNumLinks();
-		if (mbd->m_numLinks)
+	}
+	mbd->m_numLinks = this->getNumLinks();
+	if (mbd->m_numLinks)
+	{
+		int sz = sizeof(btMultiBodyLinkData);
+		int numElem = mbd->m_numLinks;
+		btChunk *chunk = serializer->allocate(sz, numElem);
+		btMultiBodyLinkData *memPtr = (btMultiBodyLinkData *)chunk->m_oldPtr;
+		for (int i = 0; i < numElem; i++, memPtr++)
 		{
-			int sz = sizeof(btMultiBodyLinkData);
-			int numElem = mbd->m_numLinks;
-			btChunk* chunk = serializer->allocate(sz,numElem);
-			btMultiBodyLinkData* memPtr = (btMultiBodyLinkData*)chunk->m_oldPtr;
-			for (int i=0;i<numElem;i++,memPtr++)
+			memPtr->m_jointType = getLink(i).m_jointType;
+			memPtr->m_dofCount = getLink(i).m_dofCount;
+			memPtr->m_posVarCount = getLink(i).m_posVarCount;
+
+			getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
+
+			getLink(i).m_absFrameTotVelocity.m_topVec.serialize(memPtr->m_absFrameTotVelocityTop);
+			getLink(i).m_absFrameTotVelocity.m_bottomVec.serialize(memPtr->m_absFrameTotVelocityBottom);
+			getLink(i).m_absFrameLocVelocity.m_topVec.serialize(memPtr->m_absFrameLocVelocityTop);
+			getLink(i).m_absFrameLocVelocity.m_bottomVec.serialize(memPtr->m_absFrameLocVelocityBottom);
+
+			memPtr->m_linkMass = getLink(i).m_mass;
+			memPtr->m_parentIndex = getLink(i).m_parent;
+			memPtr->m_jointDamping = getLink(i).m_jointDamping;
+			memPtr->m_jointFriction = getLink(i).m_jointFriction;
+			memPtr->m_jointLowerLimit = getLink(i).m_jointLowerLimit;
+			memPtr->m_jointUpperLimit = getLink(i).m_jointUpperLimit;
+			memPtr->m_jointMaxForce = getLink(i).m_jointMaxForce;
+			memPtr->m_jointMaxVelocity = getLink(i).m_jointMaxVelocity;
+
+			getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset);
+			getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
+			getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
+			btAssert(memPtr->m_dofCount <= 3);
+			for (int dof = 0; dof < getLink(i).m_dofCount; dof++)
 			{
+				getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]);
+				getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]);
 
-				memPtr->m_jointType = getLink(i).m_jointType;
-				memPtr->m_dofCount = getLink(i).m_dofCount;
-				memPtr->m_posVarCount = getLink(i).m_posVarCount;
-				
-				getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
-				memPtr->m_linkMass = getLink(i).m_mass;
-				memPtr->m_parentIndex = getLink(i).m_parent;
-				getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset);
-				getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
-				getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
-				btAssert(memPtr->m_dofCount<=3);
-				for (int dof = 0;dof<getLink(i).m_dofCount;dof++)
-				{
-					getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]);
-					getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]);
-					
-					memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof];
-					memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof];
+				memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof];
+				memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof];
+			}
+			int numPosVar = getLink(i).m_posVarCount;
+			for (int posvar = 0; posvar < numPosVar; posvar++)
+			{
+				memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar];
+			}
 
-				}
-				int numPosVar = getLink(i).m_posVarCount;
-				for (int posvar = 0; posvar < numPosVar;posvar++)
+			{
+				char *name = (char *)serializer->findNameForPointer(m_links[i].m_linkName);
+				memPtr->m_linkName = (char *)serializer->getUniquePointer(name);
+				if (memPtr->m_linkName)
 				{
-					memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar];
+					serializer->serializeName(name);
 				}
-				
-				
+			}
+			{
+				char *name = (char *)serializer->findNameForPointer(m_links[i].m_jointName);
+				memPtr->m_jointName = (char *)serializer->getUniquePointer(name);
+				if (memPtr->m_jointName)
 				{
-					char* name = (char*) serializer->findNameForPointer(m_links[i].m_linkName);
-					memPtr->m_linkName = (char*)serializer->getUniquePointer(name);
-					if (memPtr->m_linkName)
-					{
-						serializer->serializeName(name);
-					}
+					serializer->serializeName(name);
 				}
-				{
-					char* name = (char*) serializer->findNameForPointer(m_links[i].m_jointName);
-					memPtr->m_jointName = (char*)serializer->getUniquePointer(name);
-					if (memPtr->m_jointName)
-					{
-						serializer->serializeName(name);
-					}
-				}
-				memPtr->m_linkCollider = (btCollisionObjectData*)serializer->getUniquePointer(getLink(i).m_collider);
-
 			}
-			serializer->finalizeChunk(chunk,btMultiBodyLinkDataName,BT_ARRAY_CODE,(void*) &m_links[0]);
+			memPtr->m_linkCollider = (btCollisionObjectData *)serializer->getUniquePointer(getLink(i).m_collider);
 		}
-		mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
+		serializer->finalizeChunk(chunk, btMultiBodyLinkDataName, BT_ARRAY_CODE, (void *)&m_links[0]);
+	}
+	mbd->m_links = mbd->m_numLinks ? (btMultiBodyLinkData *)serializer->getUniquePointer((void *)&m_links[0]) : 0;
+
+	// Fill padding with zeros to appease msan.
+#ifdef BT_USE_DOUBLE_PRECISION
+	memset(mbd->m_padding, 0, sizeof(mbd->m_padding));
+#endif
 
-		return btMultiBodyDataName;
+	return btMultiBodyDataName;
 }
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h
index 8fbe6cda827..f2acfab9b24 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h
@@ -21,7 +21,6 @@
  
  */
 
-
 #ifndef BT_MULTIBODY_H
 #define BT_MULTIBODY_H
 
@@ -31,170 +30,193 @@
 #include "LinearMath/btMatrix3x3.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
-
 ///serialization data, don't change them if you are not familiar with the details of the serialization mechanisms
 #ifdef BT_USE_DOUBLE_PRECISION
-	#define btMultiBodyData	btMultiBodyDoubleData
-	#define btMultiBodyDataName	"btMultiBodyDoubleData"
-	#define btMultiBodyLinkData btMultiBodyLinkDoubleData
-	#define btMultiBodyLinkDataName	"btMultiBodyLinkDoubleData"
+#define btMultiBodyData btMultiBodyDoubleData
+#define btMultiBodyDataName "btMultiBodyDoubleData"
+#define btMultiBodyLinkData btMultiBodyLinkDoubleData
+#define btMultiBodyLinkDataName "btMultiBodyLinkDoubleData"
 #else
-	#define btMultiBodyData	btMultiBodyFloatData
-	#define btMultiBodyDataName	"btMultiBodyFloatData"
-	#define btMultiBodyLinkData btMultiBodyLinkFloatData
-	#define btMultiBodyLinkDataName	"btMultiBodyLinkFloatData"
-#endif //BT_USE_DOUBLE_PRECISION
+#define btMultiBodyData btMultiBodyFloatData
+#define btMultiBodyDataName "btMultiBodyFloatData"
+#define btMultiBodyLinkData btMultiBodyLinkFloatData
+#define btMultiBodyLinkDataName "btMultiBodyLinkFloatData"
+#endif  //BT_USE_DOUBLE_PRECISION
 
 #include "btMultiBodyLink.h"
 class btMultiBodyLinkCollider;
 
-ATTRIBUTE_ALIGNED16(class) btMultiBody 
+ATTRIBUTE_ALIGNED16(class)
+btMultiBody
 {
 public:
-
-    
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-    //
-    // initialization
-    //
-    
-	btMultiBody(int n_links,             // NOT including the base
-		btScalar mass,                // mass of base
-		const btVector3 &inertia,    // inertia of base, in base frame; assumed diagonal
-		bool fixedBase,           // whether the base is fixed (true) or can move (false)
-		bool canSleep, bool deprecatedMultiDof=true);
+	//
+	// initialization
+	//
 
+	btMultiBody(int n_links,               // NOT including the base
+				btScalar mass,             // mass of base
+				const btVector3 &inertia,  // inertia of base, in base frame; assumed diagonal
+				bool fixedBase,            // whether the base is fixed (true) or can move (false)
+				bool canSleep, bool deprecatedMultiDof = true);
 
 	virtual ~btMultiBody();
-    
+
 	//note: fixed link collision with parent is always disabled
-	void setupFixed(int linkIndex,
-						   btScalar mass,
-						   const btVector3 &inertia,
-						   int parent,
-						   const btQuaternion &rotParentToThis,
-						   const btVector3 &parentComToThisPivotOffset,
-                           const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true);
-
-						
+	void setupFixed(int i, //linkIndex
+					btScalar mass,
+					const btVector3 &inertia,
+					int parent,
+					const btQuaternion &rotParentToThis,
+					const btVector3 &parentComToThisPivotOffset,
+					const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision = true);
+
 	void setupPrismatic(int i,
-                               btScalar mass,
-                               const btVector3 &inertia,
-                               int parent,
-                               const btQuaternion &rotParentToThis,
-                               const btVector3 &jointAxis,
-                               const btVector3 &parentComToThisPivotOffset,
-							   const btVector3 &thisPivotToThisComOffset,
-							   bool disableParentCollision);
-
-    void setupRevolute(int linkIndex,            // 0 to num_links-1
-                       btScalar mass,
-                       const btVector3 &inertia,
-                       int parentIndex,
-                       const btQuaternion &rotParentToThis,  // rotate points in parent frame to this frame, when q = 0
-                       const btVector3 &jointAxis,    // in my frame
-                       const btVector3 &parentComToThisPivotOffset,    // vector from parent COM to joint axis, in PARENT frame
-                       const btVector3 &thisPivotToThisComOffset,       // vector from joint axis to my COM, in MY frame
-					   bool disableParentCollision=false);
-
-	void setupSpherical(int linkIndex,											// 0 to num_links-1
-                       btScalar mass,
-                       const btVector3 &inertia,
-                       int parent,
-                       const btQuaternion &rotParentToThis,		// rotate points in parent frame to this frame, when q = 0                       
-                       const btVector3 &parentComToThisPivotOffset,			// vector from parent COM to joint axis, in PARENT frame
-                       const btVector3 &thisPivotToThisComOffset,				// vector from joint axis to my COM, in MY frame
-					   bool disableParentCollision=false);		
-
-	void setupPlanar(int i,											// 0 to num_links-1
-                       btScalar mass,
-                       const btVector3 &inertia,
-                       int parent,
-                       const btQuaternion &rotParentToThis,		// rotate points in parent frame to this frame, when q = 0                       
-					   const btVector3 &rotationAxis,
-                       const btVector3 &parentComToThisComOffset,			// vector from parent COM to this COM, in PARENT frame                       
-					   bool disableParentCollision=false);		
-	
-	const btMultibodyLink& getLink(int index) const
+						btScalar mass,
+						const btVector3 &inertia,
+						int parent,
+						const btQuaternion &rotParentToThis,
+						const btVector3 &jointAxis,
+						const btVector3 &parentComToThisPivotOffset,
+						const btVector3 &thisPivotToThisComOffset,
+						bool disableParentCollision);
+
+	void setupRevolute(int i,  // 0 to num_links-1
+					   btScalar mass,
+					   const btVector3 &inertia,
+					   int parentIndex,
+					   const btQuaternion &rotParentToThis,          // rotate points in parent frame to this frame, when q = 0
+					   const btVector3 &jointAxis,                   // in my frame
+					   const btVector3 &parentComToThisPivotOffset,  // vector from parent COM to joint axis, in PARENT frame
+					   const btVector3 &thisPivotToThisComOffset,    // vector from joint axis to my COM, in MY frame
+					   bool disableParentCollision = false);
+
+	void setupSpherical(int i,  // linkIndex, 0 to num_links-1
+						btScalar mass,
+						const btVector3 &inertia,
+						int parent,
+						const btQuaternion &rotParentToThis,          // rotate points in parent frame to this frame, when q = 0
+						const btVector3 &parentComToThisPivotOffset,  // vector from parent COM to joint axis, in PARENT frame
+						const btVector3 &thisPivotToThisComOffset,    // vector from joint axis to my COM, in MY frame
+						bool disableParentCollision = false);
+
+	void setupPlanar(int i,  // 0 to num_links-1
+					 btScalar mass,
+					 const btVector3 &inertia,
+					 int parent,
+					 const btQuaternion &rotParentToThis,  // rotate points in parent frame to this frame, when q = 0
+					 const btVector3 &rotationAxis,
+					 const btVector3 &parentComToThisComOffset,  // vector from parent COM to this COM, in PARENT frame
+					 bool disableParentCollision = false);
+
+	const btMultibodyLink &getLink(int index) const
 	{
 		return m_links[index];
 	}
 
-	btMultibodyLink& getLink(int index)
+	btMultibodyLink &getLink(int index)
 	{
 		return m_links[index];
 	}
 
-
-	void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base
+	void setBaseCollider(btMultiBodyLinkCollider * collider)  //collider can be NULL to disable collision for the base
 	{
 		m_baseCollider = collider;
 	}
-	const btMultiBodyLinkCollider* getBaseCollider() const
+	const btMultiBodyLinkCollider *getBaseCollider() const
 	{
 		return m_baseCollider;
 	}
-	btMultiBodyLinkCollider* getBaseCollider()
+	btMultiBodyLinkCollider *getBaseCollider()
 	{
 		return m_baseCollider;
 	}
 
-    //
-    // get parent
-    // input: link num from 0 to num_links-1
-    // output: link num from 0 to num_links-1, OR -1 to mean the base.
-    //
-    int getParent(int link_num) const;
-    
-    
-    //
-    // get number of m_links, masses, moments of inertia
-    //
+	const btMultiBodyLinkCollider *getLinkCollider(int index) const
+	{
+		if (index >= 0 && index < getNumLinks())
+		{
+			return getLink(index).m_collider;
+		}
+		return 0;
+	}
+
+	btMultiBodyLinkCollider *getLinkCollider(int index)
+	{
+		if (index >= 0 && index < getNumLinks())
+		{
+			return getLink(index).m_collider;
+		}
+		return 0;
+	}
 
-    int getNumLinks() const { return m_links.size(); }
+	//
+	// get parent
+	// input: link num from 0 to num_links-1
+	// output: link num from 0 to num_links-1, OR -1 to mean the base.
+	//
+	int getParent(int link_num) const;
+
+	//
+	// get number of m_links, masses, moments of inertia
+	//
+
+	int getNumLinks() const { return m_links.size(); }
 	int getNumDofs() const { return m_dofCount; }
 	int getNumPosVars() const { return m_posVarCnt; }
-    btScalar getBaseMass() const { return m_baseMass; }
-    const btVector3 & getBaseInertia() const { return m_baseInertia; }
-    btScalar getLinkMass(int i) const;
-    const btVector3 & getLinkInertia(int i) const;
-    
-	
-
-    //
-    // change mass (incomplete: can only change base mass and inertia at present)
-    //
+	btScalar getBaseMass() const { return m_baseMass; }
+	const btVector3 &getBaseInertia() const { return m_baseInertia; }
+	btScalar getLinkMass(int i) const;
+	const btVector3 &getLinkInertia(int i) const;
 
-    void setBaseMass(btScalar mass) { m_baseMass = mass; }
-    void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; }
+	//
+	// change mass (incomplete: can only change base mass and inertia at present)
+	//
 
+	void setBaseMass(btScalar mass) { m_baseMass = mass; }
+	void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; }
 
-    //
-    // get/set pos/vel/rot/omega for the base link
-    //
+	//
+	// get/set pos/vel/rot/omega for the base link
+	//
 
-    const btVector3 & getBasePos() const { return m_basePos; }    // in world frame
-    const btVector3 getBaseVel() const 
-	{ 
-		return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]); 
-	}     // in world frame
-    const btQuaternion & getWorldToBaseRot() const 
+	const btVector3 &getBasePos() const 
 	{ 
-		return m_baseQuat; 
-	}     // rotates world vectors into base frame
-    btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); }   // in world frame
+		return m_basePos; 
+	}  // in world frame
+	const btVector3 getBaseVel() const
+	{
+		return btVector3(m_realBuf[3], m_realBuf[4], m_realBuf[5]);
+	}  // in world frame
+	const btQuaternion &getWorldToBaseRot() const
+	{
+		return m_baseQuat;
+	}
+    
+    const btVector3 &getInterpolateBasePos() const
+    {
+        return m_basePos_interpolate;
+    }  // in world frame
+    const btQuaternion &getInterpolateWorldToBaseRot() const
+    {
+        return m_baseQuat_interpolate;
+    }
+    
+    // rotates world vectors into base frame
+	btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); }  // in world frame
 
-    void setBasePos(const btVector3 &pos) 
-	{ 
-		m_basePos = pos; 
+	void setBasePos(const btVector3 &pos)
+	{
+		m_basePos = pos;
+        m_basePos_interpolate = pos;
 	}
 
-	void setBaseWorldTransform(const btTransform& tr)
+	void setBaseWorldTransform(const btTransform &tr)
 	{
 		setBasePos(tr.getOrigin());
 		setWorldToBaseRot(tr.getRotation().inverse());
-
 	}
 
 	btTransform getBaseWorldTransform() const
@@ -205,186 +227,212 @@ public:
 		return tr;
 	}
 
-    void setBaseVel(const btVector3 &vel) 
-	{ 
-
-		m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2]; 
+	void setBaseVel(const btVector3 &vel)
+	{
+		m_realBuf[3] = vel[0];
+		m_realBuf[4] = vel[1];
+		m_realBuf[5] = vel[2];
 	}
-    void setWorldToBaseRot(const btQuaternion &rot) 
-	{ 
-		m_baseQuat = rot;					//m_baseQuat asumed to ba alias!?
+	void setWorldToBaseRot(const btQuaternion &rot)
+	{
+		m_baseQuat = rot;  //m_baseQuat asumed to ba alias!?
+        m_baseQuat_interpolate = rot;
 	}
-    void setBaseOmega(const btVector3 &omega) 
-	{ 
-		m_realBuf[0]=omega[0]; 
-		m_realBuf[1]=omega[1]; 
-		m_realBuf[2]=omega[2]; 
+	void setBaseOmega(const btVector3 &omega)
+	{
+		m_realBuf[0] = omega[0];
+		m_realBuf[1] = omega[1];
+		m_realBuf[2] = omega[2];
 	}
 
+	//
+	// get/set pos/vel for child m_links (i = 0 to num_links-1)
+	//
 
-    //
-    // get/set pos/vel for child m_links (i = 0 to num_links-1)
-    //
-
-    btScalar getJointPos(int i) const;
-    btScalar getJointVel(int i) const;
-
-	btScalar * getJointVelMultiDof(int i);
-	btScalar * getJointPosMultiDof(int i);
+	btScalar getJointPos(int i) const;
+	btScalar getJointVel(int i) const;
 
-	const btScalar * getJointVelMultiDof(int i) const ;
-	const btScalar * getJointPosMultiDof(int i) const ;
+	btScalar *getJointVelMultiDof(int i);
+	btScalar *getJointPosMultiDof(int i);
 
-    void setJointPos(int i, btScalar q);
-    void setJointVel(int i, btScalar qdot);
-	void setJointPosMultiDof(int i, btScalar *q);
-    void setJointVelMultiDof(int i, btScalar *qdot);	
+	const btScalar *getJointVelMultiDof(int i) const;
+	const btScalar *getJointPosMultiDof(int i) const;
 
+	void setJointPos(int i, btScalar q);
+	void setJointVel(int i, btScalar qdot);
+	void setJointPosMultiDof(int i, const double *q);
+	void setJointVelMultiDof(int i, const double *qdot);
+	void setJointPosMultiDof(int i, const float *q);
+	void setJointVelMultiDof(int i, const float *qdot);
 
-
-    //
-    // direct access to velocities as a vector of 6 + num_links elements.
-    // (omega first, then v, then joint velocities.)
-    //
-    const btScalar * getVelocityVector() const 
-	{ 
-		return &m_realBuf[0]; 
+	//
+	// direct access to velocities as a vector of 6 + num_links elements.
+	// (omega first, then v, then joint velocities.)
+	//
+	const btScalar *getVelocityVector() const
+	{
+		return &m_realBuf[0];
 	}
-/*    btScalar * getVelocityVector() 
+    
+    const btScalar *getDeltaVelocityVector() const
+    {
+        return &m_deltaV[0];
+    }
+    
+    const btScalar *getSplitVelocityVector() const
+    {
+        return &m_splitV[0];
+    }
+	/*    btScalar * getVelocityVector() 
 	{ 
 		return &real_buf[0]; 
 	}
-  */  
+  */
 
-    //
-    // get the frames of reference (positions and orientations) of the child m_links
-    // (i = 0 to num_links-1)
-    //
+	//
+	// get the frames of reference (positions and orientations) of the child m_links
+	// (i = 0 to num_links-1)
+	//
 
-    const btVector3 & getRVector(int i) const;   // vector from COM(parent(i)) to COM(i), in frame i's coords
-    const btQuaternion & getParentToLocalRot(int i) const;   // rotates vectors in frame parent(i) to vectors in frame i.
+	const btVector3 &getRVector(int i) const;              // vector from COM(parent(i)) to COM(i), in frame i's coords
+	const btQuaternion &getParentToLocalRot(int i) const;  // rotates vectors in frame parent(i) to vectors in frame i.
+    const btVector3 &getInterpolateRVector(int i) const;              // vector from COM(parent(i)) to COM(i), in frame i's coords
+    const btQuaternion &getInterpolateParentToLocalRot(int i) const;  // rotates vectors in frame parent(i) to vectors in frame i.
 
+	//
+	// transform vectors in local frame of link i to world frame (or vice versa)
+	//
+	btVector3 localPosToWorld(int i, const btVector3 &local_pos) const;
+	btVector3 localDirToWorld(int i, const btVector3 &local_dir) const;
+	btVector3 worldPosToLocal(int i, const btVector3 &world_pos) const;
+	btVector3 worldDirToLocal(int i, const btVector3 &world_dir) const;
 
-    //
-    // transform vectors in local frame of link i to world frame (or vice versa)
-    //
-    btVector3 localPosToWorld(int i, const btVector3 &vec) const;
-    btVector3 localDirToWorld(int i, const btVector3 &vec) const;
-    btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
-    btVector3 worldDirToLocal(int i, const btVector3 &vec) const;
-    
+	//
+	// transform a frame in local coordinate to a frame in world coordinate
+	//
+	btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const;
 
-    //
-    // calculate kinetic energy and angular momentum
-    // useful for debugging.
-    //
 
-    btScalar getKineticEnergy() const;
-    btVector3 getAngularMomentum() const;
-    
-
-    //
-    // set external forces and torques. Note all external forces/torques are given in the WORLD frame.
-    //
+	//
+	// set external forces and torques. Note all external forces/torques are given in the WORLD frame.
+	//
 
-    void clearForcesAndTorques();
-   void clearConstraintForces();
+	void clearForcesAndTorques();
+	void clearConstraintForces();
 
 	void clearVelocities();
 
-    void addBaseForce(const btVector3 &f) 
-	{ 
-		m_baseForce += f; 
+	void addBaseForce(const btVector3 &f)
+	{
+		m_baseForce += f;
 	}
-    void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
-    void addLinkForce(int i, const btVector3 &f);
-    void addLinkTorque(int i, const btVector3 &t);
+	void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
+	void addLinkForce(int i, const btVector3 &f);
+	void addLinkTorque(int i, const btVector3 &t);
 
- void addBaseConstraintForce(const btVector3 &f)
-        {
-                m_baseConstraintForce += f;
-        }
-    void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; }
-    void addLinkConstraintForce(int i, const btVector3 &f);
-    void addLinkConstraintTorque(int i, const btVector3 &t);
-       
+	void addBaseConstraintForce(const btVector3 &f)
+	{
+		m_baseConstraintForce += f;
+	}
+	void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; }
+	void addLinkConstraintForce(int i, const btVector3 &f);
+	void addLinkConstraintTorque(int i, const btVector3 &t);
 
-void addJointTorque(int i, btScalar Q);
+	void addJointTorque(int i, btScalar Q);
 	void addJointTorqueMultiDof(int i, int dof, btScalar Q);
 	void addJointTorqueMultiDof(int i, const btScalar *Q);
 
-    const btVector3 & getBaseForce() const { return m_baseForce; }
-    const btVector3 & getBaseTorque() const { return m_baseTorque; }
-    const btVector3 & getLinkForce(int i) const;
-    const btVector3 & getLinkTorque(int i) const;
-    btScalar getJointTorque(int i) const;
-	btScalar * getJointTorqueMultiDof(int i);
-
-
-    //
-    // dynamics routines.
-    //
-
-    // timestep the velocities (given the external forces/torques set using addBaseForce etc).
-    // also sets up caches for calcAccelerationDeltas.
-    //
-    // Note: the caller must provide three vectors which are used as
-    // temporary scratch space. The idea here is to reduce dynamic
-    // memory allocation: the same scratch vectors can be re-used
-    // again and again for different Multibodies, instead of each
-    // btMultiBody allocating (and then deallocating) their own
-    // individual scratch buffers. This gives a considerable speed
-    // improvement, at least on Windows (where dynamic memory
-    // allocation appears to be fairly slow).
-    //
-   
- 
-	void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
-                        btAlignedObjectArray<btScalar> &scratch_r,
-                        btAlignedObjectArray<btVector3> &scratch_v,
-                        btAlignedObjectArray<btMatrix3x3> &scratch_m,
-			bool isConstraintPass=false
-		);
-
-///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead
-        void stepVelocitiesMultiDof(btScalar dt,
-                        btAlignedObjectArray<btScalar> &scratch_r,
-                        btAlignedObjectArray<btVector3> &scratch_v,
-                        btAlignedObjectArray<btMatrix3x3> &scratch_m,
-                        bool isConstraintPass=false)
-	{
-		computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt,scratch_r,scratch_v,scratch_m,isConstraintPass);
-        }
+	const btVector3 &getBaseForce() const { return m_baseForce; }
+	const btVector3 &getBaseTorque() const { return m_baseTorque; }
+	const btVector3 &getLinkForce(int i) const;
+	const btVector3 &getLinkTorque(int i) const;
+	btScalar getJointTorque(int i) const;
+	btScalar *getJointTorqueMultiDof(int i);
+
+	//
+	// dynamics routines.
+	//
+
+	// timestep the velocities (given the external forces/torques set using addBaseForce etc).
+	// also sets up caches for calcAccelerationDeltas.
+	//
+	// Note: the caller must provide three vectors which are used as
+	// temporary scratch space. The idea here is to reduce dynamic
+	// memory allocation: the same scratch vectors can be re-used
+	// again and again for different Multibodies, instead of each
+	// btMultiBody allocating (and then deallocating) their own
+	// individual scratch buffers. This gives a considerable speed
+	// improvement, at least on Windows (where dynamic memory
+	// allocation appears to be fairly slow).
+	//
 
-    // calcAccelerationDeltasMultiDof
-    // input: force vector (in same format as jacobian, i.e.:
-    //                      3 torque values, 3 force values, num_links joint torque values)
-    // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
-    // (existing contents of output array are replaced)
-    // calcAccelerationDeltasMultiDof must have been called first.
+	void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
+															  btAlignedObjectArray<btScalar> & scratch_r,
+															  btAlignedObjectArray<btVector3> & scratch_v,
+															  btAlignedObjectArray<btMatrix3x3> & scratch_m,
+															  bool isConstraintPass,
+                                                              bool jointFeedbackInWorldSpace,
+                                                              bool jointFeedbackInJointFrame
+                                                              );
+
+	///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead
+	//void stepVelocitiesMultiDof(btScalar dt,
+	//							btAlignedObjectArray<btScalar> & scratch_r,
+	//							btAlignedObjectArray<btVector3> & scratch_v,
+	//							btAlignedObjectArray<btMatrix3x3> & scratch_m,
+	//							bool isConstraintPass = false)
+	//{
+	//	computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false);
+	//}
+
+	// calcAccelerationDeltasMultiDof
+	// input: force vector (in same format as jacobian, i.e.:
+	//                      3 torque values, 3 force values, num_links joint torque values)
+	// output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
+	// (existing contents of output array are replaced)
+	// calcAccelerationDeltasMultiDof must have been called first.
 	void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
-                                btAlignedObjectArray<btScalar> &scratch_r,
-                                btAlignedObjectArray<btVector3> &scratch_v) const;
-	
-  
-	void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier)
+										btAlignedObjectArray<btScalar> &scratch_r,
+										btAlignedObjectArray<btVector3> &scratch_v) const;
+
+	void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
 	{
 		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
-                {
-                        m_deltaV[dof] += delta_vee[dof] * multiplier;
-                }
+		{
+			m_deltaV[dof] += delta_vee[dof] * multiplier;
+		}
 	}
+    void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
+    {
+        for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+        {
+            m_splitV[dof] += delta_vee[dof] * multiplier;
+        }
+    }
+    void addSplitV()
+    {
+        applyDeltaVeeMultiDof(&m_splitV[0], 1);
+    }
+    void substractSplitV()
+    {
+        applyDeltaVeeMultiDof(&m_splitV[0], -1);
+        
+        for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+        {
+            m_splitV[dof] = 0.f;
+        }
+    }
 	void processDeltaVeeMultiDof2()
 	{
-		applyDeltaVeeMultiDof(&m_deltaV[0],1);
+		applyDeltaVeeMultiDof(&m_deltaV[0], 1);
 
 		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
-                {
+		{
 			m_deltaV[dof] = 0.f;
 		}
 	}
 
-	void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier) 
+	void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
 	{
 		//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
 		//	printf("%.4f ", delta_vee[dof]*multiplier);
@@ -405,65 +453,81 @@ void addJointTorque(int i, btScalar Q);
 		for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
 		{
 			m_realBuf[dof] += delta_vee[dof] * multiplier;
-			btClamp(m_realBuf[dof],-m_maxCoordinateVelocity,m_maxCoordinateVelocity);
+			btClamp(m_realBuf[dof], -m_maxCoordinateVelocity, m_maxCoordinateVelocity);
 		}
-    }
+	}
 
-	
-	
-    // timestep the positions (given current velocities).
+	// timestep the positions (given current velocities).
 	void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
+    
+    // predict the positions
+    void predictPositionsMultiDof(btScalar dt);
 
+	//
+	// contacts
+	//
 
-    //
-    // contacts
-    //
+	// This routine fills out a contact constraint jacobian for this body.
+	// the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
+	// 'normal' & 'contact_point' are both given in world coordinates.
 
-    // This routine fills out a contact constraint jacobian for this body.
-    // the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
-    // 'normal' & 'contact_point' are both given in world coordinates.
-	
 	void fillContactJacobianMultiDof(int link,
-                             const btVector3 &contact_point,
-                             const btVector3 &normal,
-                             btScalar *jac,
-                             btAlignedObjectArray<btScalar> &scratch_r,
-                             btAlignedObjectArray<btVector3> &scratch_v,
-							 btAlignedObjectArray<btMatrix3x3> &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
+									 const btVector3 &contact_point,
+									 const btVector3 &normal,
+									 btScalar *jac,
+									 btAlignedObjectArray<btScalar> &scratch_r,
+									 btAlignedObjectArray<btVector3> &scratch_v,
+									 btAlignedObjectArray<btMatrix3x3> &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
 
 	//a more general version of fillContactJacobianMultiDof which does not assume..
 	//.. that the constraint in question is contact or, to be more precise, constrains linear velocity only
 	void fillConstraintJacobianMultiDof(int link,
-                             const btVector3 &contact_point,
-							 const btVector3 &normal_ang,
-                             const btVector3 &normal_lin,
-                             btScalar *jac,
-                             btAlignedObjectArray<btScalar> &scratch_r,
-                             btAlignedObjectArray<btVector3> &scratch_v,
-                             btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
-
+										const btVector3 &contact_point,
+										const btVector3 &normal_ang,
+										const btVector3 &normal_lin,
+										btScalar *jac,
+										btAlignedObjectArray<btScalar> &scratch_r,
+										btAlignedObjectArray<btVector3> &scratch_v,
+										btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
+
+	//
+	// sleeping
+	//
+	void setCanSleep(bool canSleep)
+	{
+		if (m_canWakeup)
+		{
+			m_canSleep = canSleep;
+		}
+	}
 
-    //
-    // sleeping
-    //
-	void	setCanSleep(bool canSleep)
+	bool getCanSleep() const
 	{
-		m_canSleep = canSleep;
+		return m_canSleep;
 	}
 
-	bool getCanSleep()const
+	bool getCanWakeup() const
 	{
-		return m_canSleep;
+		return m_canWakeup;
 	}
+	
+	void setCanWakeup(bool canWakeup) 
+	{
+		m_canWakeup = canWakeup;
+	}
+	bool isAwake() const { return m_awake; }
+	void wakeUp();
+	void goToSleep();
+	void checkMotionAndSleepIfRequired(btScalar timestep);
 
-    bool isAwake() const { return m_awake; }
-    void wakeUp();
-    void goToSleep();
-    void checkMotionAndSleepIfRequired(btScalar timestep);
-    
 	bool hasFixedBase() const
 	{
-		    return m_fixedBase;
+		return m_fixedBase;
+	}
+
+	void setFixedBase(bool fixedBase)
+	{
+		m_fixedBase = fixedBase;
 	}
 
 	int getCompanionId() const
@@ -476,16 +540,16 @@ void addJointTorque(int i, btScalar Q);
 		m_companionId = id;
 	}
 
-	void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links
+	void setNumLinks(int numLinks)  //careful: when changing the number of m_links, make sure to re-initialize or update existing m_links
 	{
 		m_links.resize(numLinks);
 	}
 
 	btScalar getLinearDamping() const
 	{
-			return m_linearDamping;
+		return m_linearDamping;
 	}
-	void setLinearDamping( btScalar damp)
+	void setLinearDamping(btScalar damp)
 	{
 		m_linearDamping = damp;
 	}
@@ -493,11 +557,11 @@ void addJointTorque(int i, btScalar Q);
 	{
 		return m_angularDamping;
 	}
-	void setAngularDamping( btScalar damp)
+	void setAngularDamping(btScalar damp)
 	{
 		m_angularDamping = damp;
 	}
-		
+
 	bool getUseGyroTerm() const
 	{
 		return m_useGyroTerm;
@@ -506,24 +570,24 @@ void addJointTorque(int i, btScalar Q);
 	{
 		m_useGyroTerm = useGyro;
 	}
-	btScalar	getMaxCoordinateVelocity() const
+	btScalar getMaxCoordinateVelocity() const
 	{
-		return m_maxCoordinateVelocity ;
+		return m_maxCoordinateVelocity;
 	}
-	void	setMaxCoordinateVelocity(btScalar maxVel)
+	void setMaxCoordinateVelocity(btScalar maxVel)
 	{
 		m_maxCoordinateVelocity = maxVel;
 	}
 
-	btScalar	getMaxAppliedImpulse() const
+	btScalar getMaxAppliedImpulse() const
 	{
 		return m_maxAppliedImpulse;
 	}
-	void	setMaxAppliedImpulse(btScalar maxImp)
+	void setMaxAppliedImpulse(btScalar maxImp)
 	{
 		m_maxAppliedImpulse = maxImp;
 	}
-	void	setHasSelfCollision(bool hasSelfCollision)
+	void setHasSelfCollision(bool hasSelfCollision)
 	{
 		m_hasSelfCollision = hasSelfCollision;
 	}
@@ -532,7 +596,6 @@ void addJointTorque(int i, btScalar Q);
 		return m_hasSelfCollision;
 	}
 
-	
 	void finalizeMultiDof();
 
 	void useRK4Integration(bool use) { m_useRK4 = use; }
@@ -548,213 +611,278 @@ void addJointTorque(int i, btScalar Q);
 	{
 		__posUpdated = updated;
 	}
-	
+
 	//internalNeedsJointFeedback is for internal use only
 	bool internalNeedsJointFeedback() const
 	{
 		return m_internalNeedsJointFeedback;
 	}
-	void	forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
+	void forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local, btAlignedObjectArray<btVector3> & local_origin);
 
-	void	updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
-	
-	virtual	int	calculateSerializeBufferSize()	const;
+	void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const;
+
+	void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
+    void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
+
+	virtual int calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
-	virtual	const char*	serialize(void* dataBuffer,  class btSerializer* serializer) const;
+	virtual const char *serialize(void *dataBuffer, class btSerializer *serializer) const;
 
-	const char*				getBaseName() const
+	const char *getBaseName() const
 	{
 		return m_baseName;
 	}
 	///memory of setBaseName needs to be manager by user
-	void	setBaseName(const char* name)
+	void setBaseName(const char *name)
 	{
 		m_baseName = name;
 	}
 
-private:
-    btMultiBody(const btMultiBody &);  // not implemented
-    void operator=(const btMultiBody &);  // not implemented
+	///users can point to their objects, userPointer is not used by Bullet
+	void *getUserPointer() const
+	{
+		return m_userObjectPointer;
+	}
 
-    void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
+	int getUserIndex() const
+	{
+		return m_userIndex;
+	}
 
-	void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;
-	void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
-	
-	void updateLinksDofOffsets()
+	int getUserIndex2() const
 	{
-		int dofOffset = 0, cfgOffset = 0;
-		for(int bidx = 0; bidx < m_links.size(); ++bidx)
-		{
-			m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset;
-			dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount;
-		}
+		return m_userIndex2;
+	}
+	///users can point to their objects, userPointer is not used by Bullet
+	void setUserPointer(void *userPointer)
+	{
+		m_userObjectPointer = userPointer;
 	}
 
-	void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
-	
-	
-private:
+	///users can point to their objects, userPointer is not used by Bullet
+	void setUserIndex(int index)
+	{
+		m_userIndex = index;
+	}
 
-	btMultiBodyLinkCollider* m_baseCollider;//can be NULL
-	const char*				m_baseName;//memory needs to be manager by user!
+	void setUserIndex2(int index)
+	{
+		m_userIndex2 = index;
+	}
 
-    btVector3 m_basePos;       // position of COM of base (world frame)
-    btQuaternion m_baseQuat;   // rotates world points into base frame
+	static void spatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
+		const btVector3 &displacement,     // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
+		const btVector3 &top_in,       // top part of input vector
+		const btVector3 &bottom_in,    // bottom part of input vector
+		btVector3 &top_out,         // top part of output vector
+		btVector3 &bottom_out);      // bottom part of output vector
 
-    btScalar m_baseMass;         // mass of the base
-    btVector3 m_baseInertia;   // inertia of the base (in local frame; diagonal)
 
-    btVector3 m_baseForce;     // external force applied to base. World frame.
-    btVector3 m_baseTorque;    // external torque applied to base. World frame.
-   
-    btVector3 m_baseConstraintForce;     // external force applied to base. World frame.
-    btVector3 m_baseConstraintTorque;    // external torque applied to base. World frame.
- 
-    btAlignedObjectArray<btMultibodyLink> m_links;    // array of m_links, excluding the base. index from 0 to num_links-1.
-	btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
 
-    
-    //
-    // realBuf:
-    //  offset         size            array
-    //   0              6 + num_links   v (base_omega; base_vel; joint_vels)					MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized]
-    //   6+num_links    num_links       D
-    //
-    // vectorBuf:
-    //  offset         size         array
-    //   0              num_links    h_top
-    //   num_links      num_links    h_bottom
-    //
-    // matrixBuf:
-    //  offset         size         array
-    //   0              num_links+1  rot_from_parent
-    //
-   btAlignedObjectArray<btScalar> m_deltaV; 
-    btAlignedObjectArray<btScalar> m_realBuf;
-    btAlignedObjectArray<btVector3> m_vectorBuf;
-    btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
+private:
+	btMultiBody(const btMultiBody &);     // not implemented
+	void operator=(const btMultiBody &);  // not implemented
 
+	void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const;
+	void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
+
+	void updateLinksDofOffsets()
+	{
+		int dofOffset = 0, cfgOffset = 0;
+		for (int bidx = 0; bidx < m_links.size(); ++bidx)
+		{
+			m_links[bidx].m_dofOffset = dofOffset;
+			m_links[bidx].m_cfgOffset = cfgOffset;
+			dofOffset += m_links[bidx].m_dofCount;
+			cfgOffset += m_links[bidx].m_posVarCount;
+		}
+	}
+
+	void mulMatrix(btScalar * pA, btScalar * pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
+
+private:
+	btMultiBodyLinkCollider *m_baseCollider;  //can be NULL
+	const char *m_baseName;                   //memory needs to be manager by user!
+
+	btVector3 m_basePos;      // position of COM of base (world frame)
+    btVector3 m_basePos_interpolate;      // position of interpolated COM of base (world frame)
+	btQuaternion m_baseQuat;  // rotates world points into base frame
+    btQuaternion m_baseQuat_interpolate;  
+
+	btScalar m_baseMass;      // mass of the base
+	btVector3 m_baseInertia;  // inertia of the base (in local frame; diagonal)
+
+	btVector3 m_baseForce;   // external force applied to base. World frame.
+	btVector3 m_baseTorque;  // external torque applied to base. World frame.
+
+	btVector3 m_baseConstraintForce;   // external force applied to base. World frame.
+	btVector3 m_baseConstraintTorque;  // external torque applied to base. World frame.
+
+	btAlignedObjectArray<btMultibodyLink> m_links;  // array of m_links, excluding the base. index from 0 to num_links-1.
+
+	//
+	// realBuf:
+	//  offset         size            array
+	//   0              6 + num_links   v (base_omega; base_vel; joint_vels)					MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized]
+	//   6+num_links    num_links       D
+	//
+	// vectorBuf:
+	//  offset         size         array
+	//   0              num_links    h_top
+	//   num_links      num_links    h_bottom
+	//
+	// matrixBuf:
+	//  offset         size         array
+	//   0              num_links+1  rot_from_parent
+	//
+    btAlignedObjectArray<btScalar> m_splitV;
+	btAlignedObjectArray<btScalar> m_deltaV;
+	btAlignedObjectArray<btScalar> m_realBuf;
+	btAlignedObjectArray<btVector3> m_vectorBuf;
+	btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
 
 	btMatrix3x3 m_cachedInertiaTopLeft;
 	btMatrix3x3 m_cachedInertiaTopRight;
 	btMatrix3x3 m_cachedInertiaLowerLeft;
 	btMatrix3x3 m_cachedInertiaLowerRight;
+	bool m_cachedInertiaValid;
 
-    bool m_fixedBase;
+	bool m_fixedBase;
 
-    // Sleep parameters.
-    bool m_awake;
-    bool m_canSleep;
-    btScalar m_sleepTimer;
+	// Sleep parameters.
+	bool m_awake;
+	bool m_canSleep;
+	bool m_canWakeup;
+	btScalar m_sleepTimer;
+
+	void *m_userObjectPointer;
+	int m_userIndex2;
+	int m_userIndex;
+
+	int m_companionId;
+	btScalar m_linearDamping;
+	btScalar m_angularDamping;
+	bool m_useGyroTerm;
+	btScalar m_maxAppliedImpulse;
+	btScalar m_maxCoordinateVelocity;
+	bool m_hasSelfCollision;
+
+	bool __posUpdated;
+	int m_dofCount, m_posVarCnt;
 
-	int	m_companionId;
-	btScalar	m_linearDamping;
-	btScalar	m_angularDamping;
-	bool	m_useGyroTerm;
-	btScalar	m_maxAppliedImpulse;
-	btScalar	m_maxCoordinateVelocity;
-	bool		m_hasSelfCollision;
-	
-		bool __posUpdated;
-		int m_dofCount, m_posVarCnt;
 	bool m_useRK4, m_useGlobalVelocities;
-	
+	//for global velocities, see 8.3.2B Proposed resolution in Jakub Stepien PhD Thesis
+	//https://drive.google.com/file/d/0Bz3vEa19XOYGNWdZWGpMdUdqVmZ5ZVBOaEh4ZnpNaUxxZFNV/view?usp=sharing
+
 	///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
 	bool m_internalNeedsJointFeedback;
 };
 
 struct btMultiBodyLinkDoubleData
 {
-	btQuaternionDoubleData	m_zeroRotParentToThis;
-	btVector3DoubleData		m_parentComToThisComOffset;
-	btVector3DoubleData		m_thisPivotToThisComOffset;
-	btVector3DoubleData		m_jointAxisTop[6];
-	btVector3DoubleData		m_jointAxisBottom[6];
-
-
-	char					*m_linkName;
-	char					*m_jointName;
-	btCollisionObjectDoubleData	*m_linkCollider;
-	
-	btVector3DoubleData		m_linkInertia;   // inertia of the base (in local frame; diagonal)
-	double					m_linkMass;
-	int						m_parentIndex;
-	int						m_jointType;
-	
-
-	
-
-	int						m_dofCount;
-	int						m_posVarCount;
-	double					m_jointPos[7];
-	double					m_jointVel[6];
-	double					m_jointTorque[6];
-	
-	
-	
+	btQuaternionDoubleData m_zeroRotParentToThis;
+	btVector3DoubleData m_parentComToThisPivotOffset;
+	btVector3DoubleData m_thisPivotToThisComOffset;
+	btVector3DoubleData m_jointAxisTop[6];
+	btVector3DoubleData m_jointAxisBottom[6];
+
+	btVector3DoubleData m_linkInertia;  // inertia of the base (in local frame; diagonal)
+	btVector3DoubleData m_absFrameTotVelocityTop;
+	btVector3DoubleData m_absFrameTotVelocityBottom;
+	btVector3DoubleData m_absFrameLocVelocityTop;
+	btVector3DoubleData m_absFrameLocVelocityBottom;
+
+	double m_linkMass;
+	int m_parentIndex;
+	int m_jointType;
+
+	int m_dofCount;
+	int m_posVarCount;
+	double m_jointPos[7];
+	double m_jointVel[6];
+	double m_jointTorque[6];
+
+	double m_jointDamping;
+	double m_jointFriction;
+	double m_jointLowerLimit;
+	double m_jointUpperLimit;
+	double m_jointMaxForce;
+	double m_jointMaxVelocity;
+
+	char *m_linkName;
+	char *m_jointName;
+	btCollisionObjectDoubleData *m_linkCollider;
+	char *m_paddingPtr;
 };
 
 struct btMultiBodyLinkFloatData
 {
-	btQuaternionFloatData	m_zeroRotParentToThis;
-	btVector3FloatData		m_parentComToThisComOffset;
-	btVector3FloatData		m_thisPivotToThisComOffset;
-	btVector3FloatData		m_jointAxisTop[6];
-	btVector3FloatData		m_jointAxisBottom[6];
-	
-
-	char				*m_linkName;
-	char				*m_jointName;
-	btCollisionObjectFloatData	*m_linkCollider;
-	
-	btVector3FloatData	m_linkInertia;   // inertia of the base (in local frame; diagonal)
-	int						m_dofCount;
-	float				m_linkMass;
-	int					m_parentIndex;
-	int					m_jointType;
-	
-
-		
-	float					m_jointPos[7];
-	float					m_jointVel[6];
-	float					m_jointTorque[6];
-	int						m_posVarCount;
-	
-
+	btQuaternionFloatData m_zeroRotParentToThis;
+	btVector3FloatData m_parentComToThisPivotOffset;
+	btVector3FloatData m_thisPivotToThisComOffset;
+	btVector3FloatData m_jointAxisTop[6];
+	btVector3FloatData m_jointAxisBottom[6];
+	btVector3FloatData m_linkInertia;  // inertia of the base (in local frame; diagonal)
+	btVector3FloatData m_absFrameTotVelocityTop;
+	btVector3FloatData m_absFrameTotVelocityBottom;
+	btVector3FloatData m_absFrameLocVelocityTop;
+	btVector3FloatData m_absFrameLocVelocityBottom;
+
+	int m_dofCount;
+	float m_linkMass;
+	int m_parentIndex;
+	int m_jointType;
+
+	float m_jointPos[7];
+	float m_jointVel[6];
+	float m_jointTorque[6];
+	int m_posVarCount;
+	float m_jointDamping;
+	float m_jointFriction;
+	float m_jointLowerLimit;
+	float m_jointUpperLimit;
+	float m_jointMaxForce;
+	float m_jointMaxVelocity;
+
+	char *m_linkName;
+	char *m_jointName;
+	btCollisionObjectFloatData *m_linkCollider;
+	char *m_paddingPtr;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btMultiBodyDoubleData
+struct btMultiBodyDoubleData
 {
-	char	*m_baseName;
-	btMultiBodyLinkDoubleData	*m_links;
-	btCollisionObjectDoubleData	*m_baseCollider;
-
-	btTransformDoubleData m_baseWorldTransform;
-	btVector3DoubleData m_baseInertia;   // inertia of the base (in local frame; diagonal)
-	
-	int		m_numLinks;
-	double	m_baseMass;
-
+	btVector3DoubleData m_baseWorldPosition;
+	btQuaternionDoubleData m_baseWorldOrientation;
+	btVector3DoubleData m_baseLinearVelocity;
+	btVector3DoubleData m_baseAngularVelocity;
+	btVector3DoubleData m_baseInertia;  // inertia of the base (in local frame; diagonal)
+	double m_baseMass;
+	int m_numLinks;
 	char m_padding[4];
-	
+
+	char *m_baseName;
+	btMultiBodyLinkDoubleData *m_links;
+	btCollisionObjectDoubleData *m_baseCollider;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	btMultiBodyFloatData
+struct btMultiBodyFloatData
 {
-	char	*m_baseName;
-	btMultiBodyLinkFloatData	*m_links;
-	btCollisionObjectFloatData	*m_baseCollider;
-	btTransformFloatData m_baseWorldTransform;
-	btVector3FloatData m_baseInertia;   // inertia of the base (in local frame; diagonal)
-	
-	float	m_baseMass;
-	int		m_numLinks;
+	btVector3FloatData m_baseWorldPosition;
+	btQuaternionFloatData m_baseWorldOrientation;
+	btVector3FloatData m_baseLinearVelocity;
+	btVector3FloatData m_baseAngularVelocity;
+
+	btVector3FloatData m_baseInertia;  // inertia of the base (in local frame; diagonal)
+	float m_baseMass;
+	int m_numLinks;
+
+	char *m_baseName;
+	btMultiBodyLinkFloatData *m_links;
+	btCollisionObjectFloatData *m_baseCollider;
 };
 
-
-
 #endif
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
index 12997d2e374..1ba5861145b 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
@@ -1,32 +1,30 @@
 #include "btMultiBodyConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
-#include "btMultiBodyPoint2Point.h"				//for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
-
-
-
-btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
-	:m_bodyA(bodyA),
-	m_bodyB(bodyB),
-	m_linkA(linkA),
-	m_linkB(linkB),
-	m_numRows(numRows),
-	m_jacSizeA(0),
-	m_jacSizeBoth(0),
-	m_isUnilateral(isUnilateral),
-	m_numDofsFinalized(-1),
-	m_maxAppliedImpulse(100)
+#include "btMultiBodyPoint2Point.h"  //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
+
+btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type)
+	: m_bodyA(bodyA),
+	  m_bodyB(bodyB),
+	  m_linkA(linkA),
+	  m_linkB(linkB),
+	  m_type(type),
+	  m_numRows(numRows),
+	  m_jacSizeA(0),
+	  m_jacSizeBoth(0),
+	  m_isUnilateral(isUnilateral),
+	  m_numDofsFinalized(-1),
+	  m_maxAppliedImpulse(100)
 {
-
 }
 
 void btMultiBodyConstraint::updateJacobianSizes()
 {
-    if(m_bodyA)
+	if (m_bodyA)
 	{
 		m_jacSizeA = (6 + m_bodyA->getNumDofs());
 	}
 
-	if(m_bodyB)
+	if (m_bodyB)
 	{
 		m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
 	}
@@ -38,7 +36,7 @@ void btMultiBodyConstraint::allocateJacobiansMultiDof()
 {
 	updateJacobianSizes();
 
-	m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
+	m_posOffset = ((1 + m_jacSizeBoth) * m_numRows);
 	m_data.resize((2 + m_jacSizeBoth) * m_numRows);
 }
 
@@ -46,25 +44,25 @@ btMultiBodyConstraint::~btMultiBodyConstraint()
 {
 }
 
-void	btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
+void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
 {
 	for (int i = 0; i < ndof; ++i)
-		data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
+		data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse;
 }
 
-btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstraint& solverConstraint,
-															btMultiBodyJacobianData& data,
-															btScalar* jacOrgA, btScalar* jacOrgB,
-															const btVector3& contactNormalOnB,
-															const btVector3& posAworld, const btVector3& posBworld,
-															btScalar posError,
-															const btContactSolverInfo& infoGlobal,
-															btScalar lowerLimit, btScalar upperLimit,
-															btScalar relaxation,
-															bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
+btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
+														btMultiBodyJacobianData& data,
+														btScalar* jacOrgA, btScalar* jacOrgB,
+														const btVector3& constraintNormalAng,
+														const btVector3& constraintNormalLin,
+														const btVector3& posAworld, const btVector3& posBworld,
+														btScalar posError,
+														const btContactSolverInfo& infoGlobal,
+														btScalar lowerLimit, btScalar upperLimit,
+														bool angConstraint,
+														btScalar relaxation,
+														bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
 {
-
-
 	solverConstraint.m_multiBodyA = m_bodyA;
 	solverConstraint.m_multiBodyB = m_bodyB;
 	solverConstraint.m_linkA = m_linkA;
@@ -79,7 +77,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 	btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
 	btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
 
-	btVector3 rel_pos1, rel_pos2;				//these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
+	btVector3 rel_pos1, rel_pos2;  //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
 	if (bodyA)
 		rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
 	if (bodyB)
@@ -87,121 +85,156 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 
 	if (multiBodyA)
 	{
-		if (solverConstraint.m_linkA<0)
+		if (solverConstraint.m_linkA < 0)
 		{
 			rel_pos1 = posAworld - multiBodyA->getBasePos();
-		} else
+		}
+		else
 		{
 			rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
 		}
 
-		const int ndofA  = multiBodyA->getNumDofs() + 6;
+		const int ndofA = multiBodyA->getNumDofs() + 6;
 
 		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
 
-		if (solverConstraint.m_deltaVelAindex <0)
+		if (solverConstraint.m_deltaVelAindex < 0)
 		{
 			solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
 			multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
-			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
-		} else
+			data.m_deltaVelocities.resize(data.m_deltaVelocities.size() + ndofA);
+		}
+		else
 		{
-			btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
+			btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
 		}
 
 		//determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
 		//resize..
 		solverConstraint.m_jacAindex = data.m_jacobians.size();
-		data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
+		data.m_jacobians.resize(data.m_jacobians.size() + ndofA);
 		//copy/determine
-		if(jacOrgA)
+		if (jacOrgA)
 		{
-			for (int i=0;i<ndofA;i++)
-				data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
+			for (int i = 0; i < ndofA; i++)
+				data.m_jacobians[solverConstraint.m_jacAindex + i] = jacOrgA[i];
 		}
 		else
 		{
-			btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
-			multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+			btScalar* jac1 = &data.m_jacobians[solverConstraint.m_jacAindex];
+			//multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+			multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalAng, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
 		}
 
 		//determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
 		//resize..
-		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);		//=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size() + ndofA);  //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
 		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
 		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 		//determine..
-		multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
+		multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex], delta, data.scratch_r, data.scratch_v);
 
-		btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
+		btVector3 torqueAxis0;
+		if (angConstraint)
+		{
+			torqueAxis0 = constraintNormalAng;
+		}
+		else
+		{
+			torqueAxis0 = rel_pos1.cross(constraintNormalLin);
+		}
 		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-		solverConstraint.m_contactNormal1 = contactNormalOnB;
+		solverConstraint.m_contactNormal1 = constraintNormalLin;
 	}
-	else //if(rb0)
+	else  //if(rb0)
 	{
-		btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
-		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+		btVector3 torqueAxis0;
+		if (angConstraint)
+		{
+			torqueAxis0 = constraintNormalAng;
+		}
+		else
+		{
+			torqueAxis0 = rel_pos1.cross(constraintNormalLin);
+		}
+		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
 		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-		solverConstraint.m_contactNormal1 = contactNormalOnB;
+		solverConstraint.m_contactNormal1 = constraintNormalLin;
 	}
 
 	if (multiBodyB)
 	{
-		if (solverConstraint.m_linkB<0)
+		if (solverConstraint.m_linkB < 0)
 		{
 			rel_pos2 = posBworld - multiBodyB->getBasePos();
-		} else
+		}
+		else
 		{
 			rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
 		}
 
-		const int ndofB  = multiBodyB->getNumDofs() + 6;
+		const int ndofB = multiBodyB->getNumDofs() + 6;
 
 		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
-		if (solverConstraint.m_deltaVelBindex <0)
+		if (solverConstraint.m_deltaVelBindex < 0)
 		{
 			solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
 			multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
-			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
+			data.m_deltaVelocities.resize(data.m_deltaVelocities.size() + ndofB);
 		}
 
 		//determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
 		//resize..
 		solverConstraint.m_jacBindex = data.m_jacobians.size();
-		data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
+		data.m_jacobians.resize(data.m_jacobians.size() + ndofB);
 		//copy/determine..
-		if(jacOrgB)
+		if (jacOrgB)
 		{
-			for (int i=0;i<ndofB;i++)
-				data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
+			for (int i = 0; i < ndofB; i++)
+				data.m_jacobians[solverConstraint.m_jacBindex + i] = jacOrgB[i];
 		}
 		else
 		{
-			multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+			//multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+			multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
 		}
 
 		//determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
 		//resize..
-		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size() + ndofB);
 		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
 		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 		//determine..
-		multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
+		multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex], delta, data.scratch_r, data.scratch_v);
 
-		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
+		btVector3 torqueAxis1;
+		if (angConstraint)
+		{
+			torqueAxis1 = constraintNormalAng;
+		}
+		else
+		{
+			torqueAxis1 = rel_pos2.cross(constraintNormalLin);
+		}
 		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
-		solverConstraint.m_contactNormal2 = -contactNormalOnB;
-
+		solverConstraint.m_contactNormal2 = -constraintNormalLin;
 	}
-	else //if(rb1)
+	else  //if(rb1)
 	{
-		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
-		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+		btVector3 torqueAxis1;
+		if (angConstraint)
+		{
+			torqueAxis1 = constraintNormalAng;
+		}
+		else
+		{
+			torqueAxis1 = rel_pos2.cross(constraintNormalLin);
+		}
+		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
 		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
-		solverConstraint.m_contactNormal2 = -contactNormalOnB;
+		solverConstraint.m_contactNormal2 = -constraintNormalLin;
 	}
 	{
-
 		btVector3 vec;
 		btScalar denom0 = 0.f;
 		btScalar denom1 = 0.f;
@@ -209,7 +242,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 		btScalar* jacA = 0;
 		btScalar* deltaVelA = 0;
 		btScalar* deltaVelB = 0;
-		int ndofA  = 0;
+		int ndofA = 0;
 		//determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
 		if (multiBodyA)
 		{
@@ -218,15 +251,22 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 			deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA; ++i)
 			{
-				btScalar j = jacA[i] ;
+				btScalar j = jacA[i];
 				btScalar l = deltaVelA[i];
-				denom0 += j*l;
+				denom0 += j * l;
 			}
 		}
-		else if(rb0)
+		else if (rb0)
 		{
-			vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
-			denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
+			vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
+			if (angConstraint)
+			{
+				denom0 = constraintNormalAng.dot(solverConstraint.m_angularComponentA);
+			}
+			else
+			{
+				denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec);
+			}
 		}
 		//
 		if (multiBodyB)
@@ -236,134 +276,108 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 			deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB; ++i)
 			{
-				btScalar j = jacB[i] ;
+				btScalar j = jacB[i];
 				btScalar l = deltaVelB[i];
-				denom1 += j*l;
+				denom1 += j * l;
 			}
-
 		}
-		else if(rb1)
+		else if (rb1)
 		{
-			vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
-			denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
+			vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
+			if (angConstraint)
+			{
+				denom1 = constraintNormalAng.dot(-solverConstraint.m_angularComponentB);
+			}
+			else
+			{
+				denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec);
+			}
 		}
 
 		//
-		btScalar d = denom0+denom1;
-		if (d>SIMD_EPSILON)
+		btScalar d = denom0 + denom1;
+		if (d > SIMD_EPSILON)
 		{
-			solverConstraint.m_jacDiagABInv = relaxation/(d);
+			solverConstraint.m_jacDiagABInv = relaxation / (d);
 		}
 		else
 		{
-		//disable the constraint row to handle singularity/redundant constraint
-			solverConstraint.m_jacDiagABInv  = 0.f;
+			//disable the constraint row to handle singularity/redundant constraint
+			solverConstraint.m_jacDiagABInv = 0.f;
 		}
 	}
 
-
 	//compute rhs and remaining solverConstraint fields
-	btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
+	btScalar penetration = isFriction ? 0 : posError;
 
 	btScalar rel_vel = 0.f;
-	int ndofA  = 0;
-	int ndofB  = 0;
+	int ndofA = 0;
+	int ndofB = 0;
 	{
-		btVector3 vel1,vel2;
+		btVector3 vel1, vel2;
 		if (multiBodyA)
 		{
 			ndofA = multiBodyA->getNumDofs() + 6;
 			btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
-			for (int i = 0; i < ndofA ; ++i)
+			for (int i = 0; i < ndofA; ++i)
 				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
 		}
-		else if(rb0)
+		else if (rb0)
 		{
-			rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
+			rel_vel += rb0->getLinearVelocity().dot(solverConstraint.m_contactNormal1);
+			rel_vel += rb0->getAngularVelocity().dot(solverConstraint.m_relpos1CrossNormal);
 		}
 		if (multiBodyB)
 		{
 			ndofB = multiBodyB->getNumDofs() + 6;
 			btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
-			for (int i = 0; i < ndofB ; ++i)
+			for (int i = 0; i < ndofB; ++i)
 				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
-
 		}
-		else if(rb1)
+		else if (rb1)
 		{
-			rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
+			rel_vel += rb1->getLinearVelocity().dot(solverConstraint.m_contactNormal2);
+			rel_vel += rb1->getAngularVelocity().dot(solverConstraint.m_relpos2CrossNormal);
 		}
 
-		solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
+		solverConstraint.m_friction = 0.f;  //cp.m_combinedFriction;
 	}
 
-
-	///warm starting (or zero if disabled)
-	/*
-	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-	{
-		solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-
-		if (solverConstraint.m_appliedImpulse)
-		{
-			if (multiBodyA)
-			{
-				btScalar impulse = solverConstraint.m_appliedImpulse;
-				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-				multiBodyA->applyDeltaVee(deltaV,impulse);
-				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
-			} else
-			{
-				if (rb0)
-					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
-			}
-			if (multiBodyB)
-			{
-				btScalar impulse = solverConstraint.m_appliedImpulse;
-				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-				multiBodyB->applyDeltaVee(deltaV,impulse);
-				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
-			} else
-			{
-				if (rb1)
-					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
-			}
-		}
-	} else
-	*/
-
 	solverConstraint.m_appliedImpulse = 0.f;
 	solverConstraint.m_appliedPushImpulse = 0.f;
 
 	{
-
 		btScalar positionalError = 0.f;
-		btScalar	velocityError = desiredVelocity - rel_vel;// * damping;
-
+		btScalar velocityError = desiredVelocity - rel_vel;  // * damping;
 
 		btScalar erp = infoGlobal.m_erp2;
-		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+
+		//split impulse is not implemented yet for btMultiBody*
+		//if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 		{
 			erp = infoGlobal.m_erp;
 		}
 
-		positionalError = -penetration * erp/infoGlobal.m_timeStep;
+		positionalError = -penetration * erp / infoGlobal.m_timeStep;
 
-		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
-		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+		btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
+		btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
 
-		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		//split impulse is not implemented yet for btMultiBody*
+
+		//  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 		{
 			//combine position and velocity into rhs
-			solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+			solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
 			solverConstraint.m_rhsPenetration = 0.f;
-
-		} else
-		{
-			//split position and velocity into rhs and m_rhsPenetration
-			solverConstraint.m_rhs = velocityImpulse;
-			solverConstraint.m_rhsPenetration = penetrationImpulse;
 		}
+		/*else
+        {
+            //split position and velocity into rhs and m_rhsPenetration
+            solverConstraint.m_rhs = velocityImpulse;
+            solverConstraint.m_rhsPenetration = penetrationImpulse;
+        }
+        */
 
 		solverConstraint.m_cfm = 0.f;
 		solverConstraint.m_lowerLimit = lowerLimit;
@@ -371,5 +385,4 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
 	}
 
 	return rel_vel;
-
 }
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
index 3b32b46e911..4a6007ee3ed 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
@@ -20,6 +20,21 @@ subject to the following restrictions:
 #include "LinearMath/btAlignedObjectArray.h"
 #include "btMultiBody.h"
 
+
+//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
+enum btTypedMultiBodyConstraintType
+{
+	MULTIBODY_CONSTRAINT_LIMIT=3,
+	MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR,
+	MULTIBODY_CONSTRAINT_GEAR,
+	MULTIBODY_CONSTRAINT_POINT_TO_POINT,
+	MULTIBODY_CONSTRAINT_SLIDER,
+	MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR,
+	MULTIBODY_CONSTRAINT_FIXED,
+	
+	MAX_MULTIBODY_CONSTRAINT_TYPE,
+};
+
 class btMultiBody;
 struct btSolverInfo;
 
@@ -27,152 +42,174 @@ struct btSolverInfo;
 
 struct btMultiBodyJacobianData
 {
-	btAlignedObjectArray<btScalar>		m_jacobians;
-	btAlignedObjectArray<btScalar>		m_deltaVelocitiesUnitImpulse;	//holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension
-	btAlignedObjectArray<btScalar>		m_deltaVelocities;				//holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI
-	btAlignedObjectArray<btScalar>		scratch_r;
-	btAlignedObjectArray<btVector3>		scratch_v;
-	btAlignedObjectArray<btMatrix3x3>	scratch_m;
-	btAlignedObjectArray<btSolverBody>*	m_solverBodyPool;
-	int									m_fixedBodyId;
-
+	btAlignedObjectArray<btScalar> m_jacobians;
+	btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse;  //holds the joint-space response of the corresp. tree to the test impulse in each constraint space dimension
+	btAlignedObjectArray<btScalar> m_deltaVelocities;             //holds joint-space vectors of all the constrained trees accumulating the effect of corrective impulses applied in SI
+	btAlignedObjectArray<btScalar> scratch_r;
+	btAlignedObjectArray<btVector3> scratch_v;
+	btAlignedObjectArray<btMatrix3x3> scratch_m;
+	btAlignedObjectArray<btSolverBody>* m_solverBodyPool;
+	int m_fixedBodyId;
 };
 
-
-class btMultiBodyConstraint
+ATTRIBUTE_ALIGNED16(class)
+btMultiBodyConstraint
 {
 protected:
-
-	btMultiBody*	m_bodyA;
-    btMultiBody*	m_bodyB;
-    int				m_linkA;
-    int				m_linkB;
-
-    int				m_numRows;
-    int				m_jacSizeA;
-    int				m_jacSizeBoth;
-    int				m_posOffset;
-
-	bool			m_isUnilateral;
-	int				m_numDofsFinalized;
-	btScalar		m_maxAppliedImpulse;
-
-
-    // warning: the data block lay out is not consistent for all constraints
-    // data block laid out as follows:
-    // cached impulses. (one per row.)
-    // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
-    // positions. (one per row.)
-    btAlignedObjectArray<btScalar> m_data;
-
-	void	applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
-
-	btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint,
-																btMultiBodyJacobianData& data,
-																btScalar* jacOrgA, btScalar* jacOrgB,
-																const btVector3& contactNormalOnB,
-																const btVector3& posAworld, const btVector3& posBworld,
-																btScalar posError,
-																const btContactSolverInfo& infoGlobal,
-																btScalar lowerLimit, btScalar upperLimit,
-																btScalar relaxation = 1.f,
-																bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0);
+	btMultiBody* m_bodyA;
+	btMultiBody* m_bodyB;
+	int m_linkA;
+	int m_linkB;
+
+	int m_type; //btTypedMultiBodyConstraintType
+
+	int m_numRows;
+	int m_jacSizeA;
+	int m_jacSizeBoth;
+	int m_posOffset;
+
+	bool m_isUnilateral;
+	int m_numDofsFinalized;
+	btScalar m_maxAppliedImpulse;
+
+	// warning: the data block lay out is not consistent for all constraints
+	// data block laid out as follows:
+	// cached impulses. (one per row.)
+	// jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
+	// positions. (one per row.)
+	btAlignedObjectArray<btScalar> m_data;
+
+	void applyDeltaVee(btMultiBodyJacobianData & data, btScalar * delta_vee, btScalar impulse, int velocityIndex, int ndof);
+
+	btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint & solverConstraint,
+									 btMultiBodyJacobianData & data,
+									 btScalar * jacOrgA, btScalar * jacOrgB,
+									 const btVector3& constraintNormalAng,
+
+									 const btVector3& constraintNormalLin,
+									 const btVector3& posAworld, const btVector3& posBworld,
+									 btScalar posError,
+									 const btContactSolverInfo& infoGlobal,
+									 btScalar lowerLimit, btScalar upperLimit,
+									 bool angConstraint = false,
+
+									 btScalar relaxation = 1.f,
+									 bool isFriction = false, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
 
 public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
+	btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type);
 	virtual ~btMultiBodyConstraint();
 
 	void updateJacobianSizes();
 	void allocateJacobiansMultiDof();
 
-	virtual void finalizeMultiDof()=0;
+	int getConstraintType() const
+	{
+		return m_type;
+	}
+	//many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later.
+	virtual void setFrameInB(const btMatrix3x3& frameInB) {}
+	virtual void setPivotInB(const btVector3& pivotInB) {}
 
-	virtual int getIslandIdA() const =0;
-	virtual int getIslandIdB() const =0;
+	virtual void finalizeMultiDof() = 0;
 
-	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal)=0;
+	virtual int getIslandIdA() const = 0;
+	virtual int getIslandIdB() const = 0;
 
-	int	getNumRows() const
+	virtual void createConstraintRows(btMultiBodyConstraintArray & constraintRows,
+									  btMultiBodyJacobianData & data,
+									  const btContactSolverInfo& infoGlobal) = 0;
+
+	int getNumRows() const
 	{
 		return m_numRows;
 	}
 
-	btMultiBody*	getMultiBodyA()
+	btMultiBody* getMultiBodyA()
 	{
 		return m_bodyA;
 	}
-    btMultiBody*	getMultiBodyB()
+	btMultiBody* getMultiBodyB()
 	{
 		return m_bodyB;
 	}
 
-	void	internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
+	int getLinkA() const
+	{
+		return m_linkA;
+	}
+	int getLinkB() const
+	{
+		return m_linkB;
+	}
+	void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
 	{
-		btAssert(dof>=0);
+		btAssert(dof >= 0);
 		btAssert(dof < getNumRows());
 		m_data[dof] = appliedImpulse;
 	}
-	
-	btScalar	getAppliedImpulse(int dof)
+
+	btScalar getAppliedImpulse(int dof)
 	{
-		btAssert(dof>=0);
+		btAssert(dof >= 0);
 		btAssert(dof < getNumRows());
 		return m_data[dof];
 	}
 	// current constraint position
-    // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
-    // NOTE: ignored position for friction rows.
-    btScalar getPosition(int row) const
+	// constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
+	// NOTE: ignored position for friction rows.
+	btScalar getPosition(int row) const
 	{
 		return m_data[m_posOffset + row];
 	}
 
-    void setPosition(int row, btScalar pos)
+	void setPosition(int row, btScalar pos)
 	{
 		m_data[m_posOffset + row] = pos;
 	}
 
-
 	bool isUnilateral() const
 	{
 		return m_isUnilateral;
 	}
 
 	// jacobian blocks.
-    // each of size 6 + num_links. (jacobian2 is null if no body2.)
-    // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
-    btScalar* jacobianA(int row)
+	// each of size 6 + num_links. (jacobian2 is null if no body2.)
+	// format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
+	btScalar* jacobianA(int row)
 	{
 		return &m_data[m_numRows + row * m_jacSizeBoth];
 	}
-    const btScalar* jacobianA(int row) const
+	const btScalar* jacobianA(int row) const
 	{
 		return &m_data[m_numRows + (row * m_jacSizeBoth)];
 	}
-    btScalar* jacobianB(int row)
+	btScalar* jacobianB(int row)
 	{
 		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
 	}
-    const btScalar* jacobianB(int row) const
+	const btScalar* jacobianB(int row) const
 	{
 		return &m_data[m_numRows + (row * m_jacSizeBoth) + m_jacSizeA];
 	}
 
-	btScalar	getMaxAppliedImpulse() const
+	btScalar getMaxAppliedImpulse() const
 	{
 		return m_maxAppliedImpulse;
 	}
-	void	setMaxAppliedImpulse(btScalar maxImp)
+	void setMaxAppliedImpulse(btScalar maxImp)
 	{
 		m_maxAppliedImpulse = maxImp;
 	}
 
-	virtual void debugDraw(class btIDebugDraw* drawer)=0;
+	virtual void debugDraw(class btIDebugDraw * drawer) = 0;
 
+	virtual void setGearRatio(btScalar ratio) {}
+	virtual void setGearAuxLink(int gearAuxLink) {}
+	virtual void setRelativePositionTarget(btScalar relPosTarget) {}
+	virtual void setErp(btScalar erp) {}
 };
 
-#endif //BT_MULTIBODY_CONSTRAINT_H
-
+#endif  //BT_MULTIBODY_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
index 4f66b20b2c1..2788367431e 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btMultiBodyConstraintSolver.h"
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 #include "btMultiBodyLinkCollider.h"
@@ -23,73 +22,197 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
 
 #include "LinearMath/btQuickprof.h"
+#include "BulletDynamics/Featherstone/btMultiBodySolverConstraint.h"
+#include "LinearMath/btScalar.h"
 
-btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
-	btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
-	
-	//solve featherstone non-contact constraints
+	btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
+	//solve featherstone non-contact constraints
+	btScalar nonContactResidual = 0;
 	//printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
-	for (int j=0;j<m_multiBodyNonContactConstraints.size();j++)
+	for (int i = 0; i < infoGlobal.m_numNonContactInnerIterations; ++i)
 	{
-		btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j];
-		
-		resolveSingleConstraintRowGeneric(constraint);
-		if(constraint.m_multiBodyA) 
-			constraint.m_multiBodyA->setPosUpdated(false);
-		if(constraint.m_multiBodyB) 
-			constraint.m_multiBodyB->setPosUpdated(false);
+		// reset the nonContactResdual to 0 at start of each inner iteration
+		nonContactResidual = 0;
+		for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
+		{
+			int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
+
+			btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
+
+			btScalar residual = resolveSingleConstraintRowGeneric(constraint);
+			nonContactResidual = btMax(nonContactResidual, residual * residual);
+
+			if (constraint.m_multiBodyA)
+				constraint.m_multiBodyA->setPosUpdated(false);
+			if (constraint.m_multiBodyB)
+				constraint.m_multiBodyB->setPosUpdated(false);
+		}
 	}
+	leastSquaredResidual = btMax(leastSquaredResidual, nonContactResidual);
 
 	//solve featherstone normal contact
-	for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++)
+	for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++)
 	{
-		btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
+		int index = j0;  //iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0;
+
+		btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[index];
+		btScalar residual = 0.f;
+
 		if (iteration < infoGlobal.m_numIterations)
-			resolveSingleConstraintRowGeneric(constraint);
+		{
+			residual = resolveSingleConstraintRowGeneric(constraint);
+		}
+
+		leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
 
-		if(constraint.m_multiBodyA) 
+		if (constraint.m_multiBodyA)
 			constraint.m_multiBodyA->setPosUpdated(false);
-		if(constraint.m_multiBodyB) 
+		if (constraint.m_multiBodyB)
 			constraint.m_multiBodyB->setPosUpdated(false);
 	}
-	
+
 	//solve featherstone frictional contact
+	if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0))
+	{
+		for (int j1 = 0; j1 < this->m_multiBodySpinningFrictionContactConstraints.size(); j1++)
+		{
+			if (iteration < infoGlobal.m_numIterations)
+			{
+				int index = j1;
+
+				btMultiBodySolverConstraint& frictionConstraint = m_multiBodySpinningFrictionContactConstraints[index];
+				btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
+				//adjust friction limits here
+				if (totalImpulse > btScalar(0))
+				{
+					frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
+					frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
+					btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
+					leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
+
+					if (frictionConstraint.m_multiBodyA)
+						frictionConstraint.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraint.m_multiBodyB)
+						frictionConstraint.m_multiBodyB->setPosUpdated(false);
+				}
+			}
+		}
+
+		for (int j1 = 0; j1 < this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++)
+		{
+			if (iteration < infoGlobal.m_numIterations)
+			{
+				int index = j1;  //iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1;
+
+				btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index];
+				btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
+				j1++;
+				int index2 = j1;
+				btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyTorsionalFrictionContactConstraints[index2];
+				//adjust friction limits here
+				if (totalImpulse > btScalar(0) && frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
+				{
+					frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
+					frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
+					frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse);
+					frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse;
+
+					btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
+					leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
+
+					if (frictionConstraint.m_multiBodyA)
+						frictionConstraint.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraint.m_multiBodyB)
+						frictionConstraint.m_multiBodyB->setPosUpdated(false);
+
+					if (frictionConstraintB.m_multiBodyA)
+						frictionConstraintB.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraintB.m_multiBodyB)
+						frictionConstraintB.m_multiBodyB->setPosUpdated(false);
+				}
+			}
+		}
+
+		for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++)
+		{
+			if (iteration < infoGlobal.m_numIterations)
+			{
+				int index = j1;  //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
+				btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
 
-	for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++)
+				btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
+				j1++;
+				int index2 = j1;  //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
+				btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyFrictionContactConstraints[index2];
+				btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex);
+
+				if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
+				{
+					frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
+					frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
+					frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse);
+					frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse;
+					btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
+					leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
+
+					if (frictionConstraintB.m_multiBodyA)
+						frictionConstraintB.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraintB.m_multiBodyB)
+						frictionConstraintB.m_multiBodyB->setPosUpdated(false);
+
+					if (frictionConstraint.m_multiBodyA)
+						frictionConstraint.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraint.m_multiBodyB)
+						frictionConstraint.m_multiBodyB->setPosUpdated(false);
+				}
+			}
+		}
+	}
+	else
 	{
-		if (iteration < infoGlobal.m_numIterations)
+		for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++)
 		{
-			btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j];
-			btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
-			//adjust friction limits here
-			if (totalImpulse>btScalar(0))
+			if (iteration < infoGlobal.m_numIterations)
 			{
-				frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
-				frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
-				resolveSingleConstraintRowGeneric(frictionConstraint);
-
-				if(frictionConstraint.m_multiBodyA) 
-					frictionConstraint.m_multiBodyA->setPosUpdated(false);
-				if(frictionConstraint.m_multiBodyB) 
-					frictionConstraint.m_multiBodyB->setPosUpdated(false);
+				int index = j1;  //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
+
+				btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
+				btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
+				//adjust friction limits here
+				if (totalImpulse > btScalar(0))
+				{
+					frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
+					frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
+					btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
+					leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
+
+					if (frictionConstraint.m_multiBodyA)
+						frictionConstraint.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraint.m_multiBodyB)
+						frictionConstraint.m_multiBodyB->setPosUpdated(false);
+				}
 			}
 		}
 	}
-	return val;
+	return leastSquaredResidual;
 }
 
-btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	m_multiBodyNonContactConstraints.resize(0);
 	m_multiBodyNormalContactConstraints.resize(0);
 	m_multiBodyFrictionContactConstraints.resize(0);
+	m_multiBodyTorsionalFrictionContactConstraints.resize(0);
+	m_multiBodySpinningFrictionContactConstraints.resize(0);
+
 	m_data.m_jacobians.resize(0);
 	m_data.m_deltaVelocitiesUnitImpulse.resize(0);
 	m_data.m_deltaVelocities.resize(0);
 
-	for (int i=0;i<numBodies;i++)
+	for (int i = 0; i < numBodies; i++)
 	{
 		const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]);
 		if (fcA)
@@ -98,63 +221,63 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb
 		}
 	}
 
-	btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
+	btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
 	return val;
 }
 
-void	btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
+void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
 {
-    for (int i = 0; i < ndof; ++i) 
-		m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
+	for (int i = 0; i < ndof; ++i)
+		m_data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse;
 }
 
-void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
+btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
 {
-
-	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
-	btScalar deltaVelADotn=0;
-	btScalar deltaVelBDotn=0;
+	btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm;
+	btScalar deltaVelADotn = 0;
+	btScalar deltaVelBDotn = 0;
 	btSolverBody* bodyA = 0;
 	btSolverBody* bodyB = 0;
-	int ndofA=0;
-	int ndofB=0;
+	int ndofA = 0;
+	int ndofB = 0;
 
 	if (c.m_multiBodyA)
 	{
-		ndofA  = c.m_multiBodyA->getNumDofs() + 6;
-		for (int i = 0; i < ndofA; ++i) 
-			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
-	} else if(c.m_solverBodyIdA >= 0)
+		ndofA = c.m_multiBodyA->getNumDofs() + 6;
+		for (int i = 0; i < ndofA; ++i)
+			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex + i] * m_data.m_deltaVelocities[c.m_deltaVelAindex + i];
+	}
+	else if (c.m_solverBodyIdA >= 0)
 	{
 		bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
-		deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
+		deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
 	}
 
 	if (c.m_multiBodyB)
 	{
-		ndofB  = c.m_multiBodyB->getNumDofs() + 6;
-		for (int i = 0; i < ndofB; ++i) 
-			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
-	} else if(c.m_solverBodyIdB >= 0)
+		ndofB = c.m_multiBodyB->getNumDofs() + 6;
+		for (int i = 0; i < ndofB; ++i)
+			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex + i] * m_data.m_deltaVelocities[c.m_deltaVelBindex + i];
+	}
+	else if (c.m_solverBodyIdB >= 0)
 	{
 		bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
-		deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity())  + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
+		deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
 	}
 
-	
-	deltaImpulse	-=	deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
-	deltaImpulse	-=	deltaVelBDotn*c.m_jacDiagABInv;
+	deltaImpulse -= deltaVelADotn * c.m_jacDiagABInv;  //m_jacDiagABInv = 1./denom
+	deltaImpulse -= deltaVelBDotn * c.m_jacDiagABInv;
 	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
-	
+
 	if (sum < c.m_lowerLimit)
 	{
-		deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
+		deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
 		c.m_appliedImpulse = c.m_lowerLimit;
 	}
-	else if (sum > c.m_upperLimit) 
+	else if (sum > c.m_upperLimit)
 	{
-		deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
+		deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
 		c.m_appliedImpulse = c.m_upperLimit;
 	}
 	else
@@ -164,42 +287,219 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 
 	if (c.m_multiBodyA)
 	{
-		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
-		c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
-#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
-	} else if(c.m_solverBodyIdA >= 0)
+		c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse);
+#endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	}
+	else if (c.m_solverBodyIdA >= 0)
 	{
-		bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
-
+		bodyA->internalApplyImpulse(c.m_contactNormal1 * bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
 	}
 	if (c.m_multiBodyB)
 	{
-		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB);
 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
-		c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
-#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
-	} else if(c.m_solverBodyIdB >= 0)
+		c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse);
+#endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	}
+	else if (c.m_solverBodyIdB >= 0)
 	{
-		bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+		bodyB->internalApplyImpulse(c.m_contactNormal2 * bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
 	}
-
+	btScalar deltaVel = deltaImpulse / c.m_jacDiagABInv;
+	return deltaVel;
 }
 
+btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB)
+{
+	int ndofA = 0;
+	int ndofB = 0;
+	btSolverBody* bodyA = 0;
+	btSolverBody* bodyB = 0;
+	btScalar deltaImpulseB = 0.f;
+	btScalar sumB = 0.f;
+	{
+		deltaImpulseB = cB.m_rhs - btScalar(cB.m_appliedImpulse) * cB.m_cfm;
+		btScalar deltaVelADotn = 0;
+		btScalar deltaVelBDotn = 0;
+		if (cB.m_multiBodyA)
+		{
+			ndofA = cB.m_multiBodyA->getNumDofs() + 6;
+			for (int i = 0; i < ndofA; ++i)
+				deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex + i];
+		}
+		else if (cB.m_solverBodyIdA >= 0)
+		{
+			bodyA = &m_tmpSolverBodyPool[cB.m_solverBodyIdA];
+			deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
+		}
 
+		if (cB.m_multiBodyB)
+		{
+			ndofB = cB.m_multiBodyB->getNumDofs() + 6;
+			for (int i = 0; i < ndofB; ++i)
+				deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex + i];
+		}
+		else if (cB.m_solverBodyIdB >= 0)
+		{
+			bodyB = &m_tmpSolverBodyPool[cB.m_solverBodyIdB];
+			deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
+		}
+
+		deltaImpulseB -= deltaVelADotn * cB.m_jacDiagABInv;  //m_jacDiagABInv = 1./denom
+		deltaImpulseB -= deltaVelBDotn * cB.m_jacDiagABInv;
+		sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB;
+	}
+
+	btScalar deltaImpulseA = 0.f;
+	btScalar sumA = 0.f;
+	const btMultiBodySolverConstraint& cA = cA1;
+	{
+		{
+			deltaImpulseA = cA.m_rhs - btScalar(cA.m_appliedImpulse) * cA.m_cfm;
+			btScalar deltaVelADotn = 0;
+			btScalar deltaVelBDotn = 0;
+			if (cA.m_multiBodyA)
+			{
+				ndofA = cA.m_multiBodyA->getNumDofs() + 6;
+				for (int i = 0; i < ndofA; ++i)
+					deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex + i];
+			}
+			else if (cA.m_solverBodyIdA >= 0)
+			{
+				bodyA = &m_tmpSolverBodyPool[cA.m_solverBodyIdA];
+				deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
+			}
+
+			if (cA.m_multiBodyB)
+			{
+				ndofB = cA.m_multiBodyB->getNumDofs() + 6;
+				for (int i = 0; i < ndofB; ++i)
+					deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex + i];
+			}
+			else if (cA.m_solverBodyIdB >= 0)
+			{
+				bodyB = &m_tmpSolverBodyPool[cA.m_solverBodyIdB];
+				deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
+			}
+
+			deltaImpulseA -= deltaVelADotn * cA.m_jacDiagABInv;  //m_jacDiagABInv = 1./denom
+			deltaImpulseA -= deltaVelBDotn * cA.m_jacDiagABInv;
+			sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA;
+		}
+	}
+
+	if (sumA * sumA + sumB * sumB >= cA.m_lowerLimit * cB.m_lowerLimit)
+	{
+		btScalar angle = btAtan2(sumA, sumB);
+		btScalar sumAclipped = btFabs(cA.m_lowerLimit * btSin(angle));
+		btScalar sumBclipped = btFabs(cB.m_lowerLimit * btCos(angle));
+
+		if (sumA < -sumAclipped)
+		{
+			deltaImpulseA = -sumAclipped - cA.m_appliedImpulse;
+			cA.m_appliedImpulse = -sumAclipped;
+		}
+		else if (sumA > sumAclipped)
+		{
+			deltaImpulseA = sumAclipped - cA.m_appliedImpulse;
+			cA.m_appliedImpulse = sumAclipped;
+		}
+		else
+		{
+			cA.m_appliedImpulse = sumA;
+		}
+
+		if (sumB < -sumBclipped)
+		{
+			deltaImpulseB = -sumBclipped - cB.m_appliedImpulse;
+			cB.m_appliedImpulse = -sumBclipped;
+		}
+		else if (sumB > sumBclipped)
+		{
+			deltaImpulseB = sumBclipped - cB.m_appliedImpulse;
+			cB.m_appliedImpulse = sumBclipped;
+		}
+		else
+		{
+			cB.m_appliedImpulse = sumB;
+		}
+		//deltaImpulseA = sumAclipped-cA.m_appliedImpulse;
+		//cA.m_appliedImpulse = sumAclipped;
+		//deltaImpulseB = sumBclipped-cB.m_appliedImpulse;
+		//cB.m_appliedImpulse = sumBclipped;
+	}
+	else
+	{
+		cA.m_appliedImpulse = sumA;
+		cB.m_appliedImpulse = sumB;
+	}
+
+	if (cA.m_multiBodyA)
+	{
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA, cA.m_deltaVelAindex, ndofA);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA);
+#endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	}
+	else if (cA.m_solverBodyIdA >= 0)
+	{
+		bodyA->internalApplyImpulse(cA.m_contactNormal1 * bodyA->internalGetInvMass(), cA.m_angularComponentA, deltaImpulseA);
+	}
+	if (cA.m_multiBodyB)
+	{
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA, cA.m_deltaVelBindex, ndofB);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA);
+#endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	}
+	else if (cA.m_solverBodyIdB >= 0)
+	{
+		bodyB->internalApplyImpulse(cA.m_contactNormal2 * bodyB->internalGetInvMass(), cA.m_angularComponentB, deltaImpulseA);
+	}
+
+	if (cB.m_multiBodyA)
+	{
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB, cB.m_deltaVelAindex, ndofA);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB);
+#endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	}
+	else if (cB.m_solverBodyIdA >= 0)
+	{
+		bodyA->internalApplyImpulse(cB.m_contactNormal1 * bodyA->internalGetInvMass(), cB.m_angularComponentA, deltaImpulseB);
+	}
+	if (cB.m_multiBodyB)
+	{
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB, cB.m_deltaVelBindex, ndofB);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+		//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+		cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB);
+#endif  //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+	}
+	else if (cB.m_solverBodyIdB >= 0)
+	{
+		bodyB->internalApplyImpulse(cB.m_contactNormal2 * bodyB->internalGetInvMass(), cB.m_angularComponentB, deltaImpulseB);
+	}
 
+	btScalar deltaVel = deltaImpulseA / cA.m_jacDiagABInv + deltaImpulseB / cB.m_jacDiagABInv;
+	return deltaVel;
+}
 
-void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, 
-																 const btVector3& contactNormal,
-																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
-																 btScalar& relaxation,
-																 bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
+void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& contactNormal, const btScalar& appliedImpulse, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
 {
-			
 	BT_PROFILE("setupMultiBodyContactConstraint");
 	btVector3 rel_pos1;
 	btVector3 rel_pos2;
@@ -217,214 +517,266 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 	btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
 
 	if (bodyA)
-		rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
+		rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
 	if (bodyB)
 		rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
 
-	relaxation = 1.f;
+	relaxation = infoGlobal.m_sor;
 
-	
+	btScalar invTimeStep = btScalar(1) / infoGlobal.m_timeStep;
+
+	//cfm = 1 /       ( dt * kp + kd )
+	//erp = dt * kp / ( dt * kp + kd )
 
+	btScalar cfm;
+	btScalar erp;
+	if (isFriction)
+	{
+		cfm = infoGlobal.m_frictionCFM;
+		erp = infoGlobal.m_frictionERP;
+	}
+	else
+	{
+		cfm = infoGlobal.m_globalCfm;
+		erp = infoGlobal.m_erp2;
+
+		if ((cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP))
+		{
+			if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM)
+				cfm = cp.m_contactCFM;
+			if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP)
+				erp = cp.m_contactERP;
+		}
+		else
+		{
+			if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
+			{
+				btScalar denom = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1);
+				if (denom < SIMD_EPSILON)
+				{
+					denom = SIMD_EPSILON;
+				}
+				cfm = btScalar(1) / denom;
+				erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
+			}
+		}
+	}
+
+	cfm *= invTimeStep;
 
 	if (multiBodyA)
 	{
-		if (solverConstraint.m_linkA<0)
+		if (solverConstraint.m_linkA < 0)
 		{
 			rel_pos1 = pos1 - multiBodyA->getBasePos();
-		} else
+		}
+		else
 		{
 			rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
 		}
-		const int ndofA  = multiBodyA->getNumDofs() + 6;
+		const int ndofA = multiBodyA->getNumDofs() + 6;
 
 		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
 
-		if (solverConstraint.m_deltaVelAindex <0)
+		if (solverConstraint.m_deltaVelAindex < 0)
 		{
 			solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
 			multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
-			m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA);
-		} else
+			m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofA);
+		}
+		else
 		{
-			btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
+			btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
 		}
 
 		solverConstraint.m_jacAindex = m_data.m_jacobians.size();
-		m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA);
-		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
+		m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofA);
+		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofA);
 		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
 
-		btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
+		btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 		multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
 		btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-		multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
+		multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v);
 
 		btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
 		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
 		solverConstraint.m_contactNormal1 = contactNormal;
-	} else
+	}
+	else
 	{
 		btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
 		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
 		solverConstraint.m_contactNormal1 = contactNormal;
-		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
 	}
 
-	
-
 	if (multiBodyB)
 	{
-		if (solverConstraint.m_linkB<0)
+		if (solverConstraint.m_linkB < 0)
 		{
 			rel_pos2 = pos2 - multiBodyB->getBasePos();
-		} else
+		}
+		else
 		{
 			rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
 		}
 
-		const int ndofB  = multiBodyB->getNumDofs() + 6;
+		const int ndofB = multiBodyB->getNumDofs() + 6;
 
 		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
-		if (solverConstraint.m_deltaVelBindex <0)
+		if (solverConstraint.m_deltaVelBindex < 0)
 		{
 			solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
 			multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
-			m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB);
+			m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofB);
 		}
 
 		solverConstraint.m_jacBindex = m_data.m_jacobians.size();
 
-		m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB);
-		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
+		m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofB);
+		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofB);
 		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
 
 		multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
-		multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
-		
-		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);		
+		multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v);
+
+		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
 		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
 		solverConstraint.m_contactNormal2 = -contactNormal;
-	
-	} else
+	}
+	else
 	{
-		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);		
+		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
 		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
 		solverConstraint.m_contactNormal2 = -contactNormal;
-	
-		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+
+		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
 	}
 
 	{
-						
 		btVector3 vec;
 		btScalar denom0 = 0.f;
 		btScalar denom1 = 0.f;
 		btScalar* jacB = 0;
 		btScalar* jacA = 0;
-		btScalar* lambdaA =0;
-		btScalar* lambdaB =0;
-		int ndofA  = 0;
+		btScalar* lambdaA = 0;
+		btScalar* lambdaB = 0;
+		int ndofA = 0;
 		if (multiBodyA)
 		{
-			ndofA  = multiBodyA->getNumDofs() + 6;
+			ndofA = multiBodyA->getNumDofs() + 6;
 			jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 			lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA; ++i)
 			{
-				btScalar j = jacA[i] ;
-				btScalar l =lambdaA[i];
-				denom0 += j*l;
+				btScalar j = jacA[i];
+				btScalar l = lambdaA[i];
+				denom0 += j * l;
 			}
-		} else
+		}
+		else
 		{
 			if (rb0)
 			{
-				vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+				vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
 				denom0 = rb0->getInvMass() + contactNormal.dot(vec);
 			}
 		}
 		if (multiBodyB)
 		{
-			const int ndofB  = multiBodyB->getNumDofs() + 6;
+			const int ndofB = multiBodyB->getNumDofs() + 6;
 			jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
 			lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB; ++i)
 			{
-				btScalar j = jacB[i] ;
-				btScalar l =lambdaB[i];
-				denom1 += j*l;
+				btScalar j = jacB[i];
+				btScalar l = lambdaB[i];
+				denom1 += j * l;
 			}
-
-		} else
+		}
+		else
 		{
 			if (rb1)
 			{
-				vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+				vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
 				denom1 = rb1->getInvMass() + contactNormal.dot(vec);
 			}
 		}
 
-		 
-
-		 btScalar d = denom0+denom1;
-		 if (d>SIMD_EPSILON)
-		 {
-			solverConstraint.m_jacDiagABInv = relaxation/(d);
-		 } else
-		 {
+		btScalar d = denom0 + denom1 + cfm;
+		if (d > SIMD_EPSILON)
+		{
+			solverConstraint.m_jacDiagABInv = relaxation / (d);
+		}
+		else
+		{
 			//disable the constraint row to handle singularity/redundant constraint
-			solverConstraint.m_jacDiagABInv  = 0.f;
-		 }
-		
+			solverConstraint.m_jacDiagABInv = 0.f;
+		}
 	}
 
-	
 	//compute rhs and remaining solverConstraint fields
 
-	
-
 	btScalar restitution = 0.f;
-	btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
+	btScalar distance = 0;
+	if (!isFriction)
+	{
+		distance = cp.getDistance() + infoGlobal.m_linearSlop;
+	}
+	else
+	{
+		if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
+		{
+			distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal);
+		}
+	}
 
 	btScalar rel_vel = 0.f;
-	int ndofA  = 0;
-	int ndofB  = 0;
+	int ndofA = 0;
+	int ndofB = 0;
 	{
-
-		btVector3 vel1,vel2;
+		btVector3 vel1, vel2;
 		if (multiBodyA)
 		{
-			ndofA  = multiBodyA->getNumDofs() + 6;
+			ndofA = multiBodyA->getNumDofs() + 6;
 			btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
-			for (int i = 0; i < ndofA ; ++i) 
+			for (int i = 0; i < ndofA; ++i)
 				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
-		} else
+		}
+		else
 		{
 			if (rb0)
 			{
-				rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
+				rel_vel += (rb0->getVelocityInLocalPoint(rel_pos1) +
+							(rb0->getTotalTorque() * rb0->getInvInertiaTensorWorld() * infoGlobal.m_timeStep).cross(rel_pos1) +
+							rb0->getTotalForce() * rb0->getInvMass() * infoGlobal.m_timeStep)
+							   .dot(solverConstraint.m_contactNormal1);
 			}
 		}
 		if (multiBodyB)
 		{
-			ndofB  = multiBodyB->getNumDofs() + 6;
+			ndofB = multiBodyB->getNumDofs() + 6;
 			btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
-			for (int i = 0; i < ndofB ; ++i) 
+			for (int i = 0; i < ndofB; ++i)
 				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
-
-		} else
+		}
+		else
 		{
 			if (rb1)
 			{
-				rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
+				rel_vel += (rb1->getVelocityInLocalPoint(rel_pos2) +
+							(rb1->getTotalTorque() * rb1->getInvInertiaTensorWorld() * infoGlobal.m_timeStep).cross(rel_pos2) +
+							rb1->getTotalForce() * rb1->getInvMass() * infoGlobal.m_timeStep)
+							   .dot(solverConstraint.m_contactNormal2);
 			}
 		}
 
 		solverConstraint.m_friction = cp.m_combinedFriction;
 
-		if(!isFriction)
+		if (!isFriction)
 		{
-			restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);	
+			restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
 			if (restitution <= btScalar(0.))
 			{
 				restitution = 0.f;
@@ -432,12 +784,70 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		}
 	}
 
+	{
+		btScalar positionalError = 0.f;
+		btScalar velocityError = restitution - rel_vel;  // * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
+		if (isFriction)
+		{
+			positionalError = -distance * erp / infoGlobal.m_timeStep;
+		}
+		else
+		{
+			if (distance > 0)
+			{
+				positionalError = 0;
+				velocityError -= distance / infoGlobal.m_timeStep;
+			}
+			else
+			{
+				positionalError = -distance * erp / infoGlobal.m_timeStep;
+			}
+		}
+
+		btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
+		btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
+
+		if (!isFriction)
+		{
+			//	if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+			{
+				//combine position and velocity into rhs
+				solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
+				solverConstraint.m_rhsPenetration = 0.f;
+			}
+			/*else
+			{
+				//split position and velocity into rhs and m_rhsPenetration
+				solverConstraint.m_rhs = velocityImpulse;
+				solverConstraint.m_rhsPenetration = penetrationImpulse;
+			}
+			*/
+			solverConstraint.m_lowerLimit = 0;
+			solverConstraint.m_upperLimit = 1e10f;
+		}
+		else
+		{
+			solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
+			solverConstraint.m_rhsPenetration = 0.f;
+			solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
+			solverConstraint.m_upperLimit = solverConstraint.m_friction;
+		}
 
-	///warm starting (or zero if disabled)
-	//disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
-	if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+		solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv;
+	}
+        
+	if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING)
 	{
-		solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+		if (btFabs(cp.m_prevRHS) > 1e-5 && cp.m_prevRHS < 2* solverConstraint.m_rhs && solverConstraint.m_rhs < 2*cp.m_prevRHS)
+		{
+			solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse / cp.m_prevRHS * solverConstraint.m_rhs * infoGlobal.m_articulatedWarmstartingFactor;
+			if (solverConstraint.m_appliedImpulse < 0)
+				solverConstraint.m_appliedImpulse = 0;
+		}
+		else
+		{
+			solverConstraint.m_appliedImpulse = 0.f;
+		}
 
 		if (solverConstraint.m_appliedImpulse)
 		{
@@ -445,109 +855,348 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			{
 				btScalar impulse = solverConstraint.m_appliedImpulse;
 				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-				multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
-				
-				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
-			} else
+				multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
+
+				applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
+			}
+			else
 			{
 				if (rb0)
-					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
 			}
 			if (multiBodyB)
 			{
 				btScalar impulse = solverConstraint.m_appliedImpulse;
 				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-				multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
-				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
-			} else
+				multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
+				applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
+			}
+			else
 			{
 				if (rb1)
-					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
+					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
 			}
 		}
-	} else
+	}
+	else
 	{
 		solverConstraint.m_appliedImpulse = 0.f;
+   	solverConstraint.m_appliedPushImpulse = 0.f;
 	}
+}
 
-	solverConstraint.m_appliedPushImpulse = 0.f;
+void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
+																			const btVector3& constraintNormal,
+																			btManifoldPoint& cp,
+																			btScalar combinedTorsionalFriction,
+																			const btContactSolverInfo& infoGlobal,
+																			btScalar& relaxation,
+																			bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
+{
+	BT_PROFILE("setupMultiBodyRollingFrictionConstraint");
+	btVector3 rel_pos1;
+	btVector3 rel_pos2;
+
+	btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
+	btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
+
+	const btVector3& pos1 = cp.getPositionWorldOnA();
+	const btVector3& pos2 = cp.getPositionWorldOnB();
+
+	btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
+	btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
+
+	btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
+	btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
 
+	if (bodyA)
+		rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
+	if (bodyB)
+		rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
+
+	relaxation = infoGlobal.m_sor;
+
+	// btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
+
+	if (multiBodyA)
 	{
+		if (solverConstraint.m_linkA < 0)
+		{
+			rel_pos1 = pos1 - multiBodyA->getBasePos();
+		}
+		else
+		{
+			rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
+		}
+		const int ndofA = multiBodyA->getNumDofs() + 6;
 
-		btScalar positionalError = 0.f;
-		btScalar velocityError = restitution - rel_vel;// * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
+		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
 
-		btScalar erp = infoGlobal.m_erp2;
-		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		if (solverConstraint.m_deltaVelAindex < 0)
+		{
+			solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
+			multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
+			m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofA);
+		}
+		else
 		{
-			erp = infoGlobal.m_erp;
+			btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
 		}
 
-		if (penetration>0)
+		solverConstraint.m_jacAindex = m_data.m_jacobians.size();
+		m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofA);
+		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofA);
+		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
+
+		btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex];
+		multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, btVector3(0, 0, 0), jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+		btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+		multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v);
+
+		btVector3 torqueAxis0 = constraintNormal;
+		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+		solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
+	}
+	else
+	{
+		btVector3 torqueAxis0 = constraintNormal;
+		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+		solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
+		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
+	}
+
+	if (multiBodyB)
+	{
+		if (solverConstraint.m_linkB < 0)
 		{
-			positionalError = 0;
-			velocityError -= penetration / infoGlobal.m_timeStep;
+			rel_pos2 = pos2 - multiBodyB->getBasePos();
+		}
+		else
+		{
+			rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
+		}
+
+		const int ndofB = multiBodyB->getNumDofs() + 6;
 
-		} else
+		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
+		if (solverConstraint.m_deltaVelBindex < 0)
 		{
-			positionalError = -penetration * erp/infoGlobal.m_timeStep;
+			solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
+			multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
+			m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofB);
 		}
 
-		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
-		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+		solverConstraint.m_jacBindex = m_data.m_jacobians.size();
+
+		m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofB);
+		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofB);
+		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
+
+		multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0, 0, 0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+		multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v);
+
+		btVector3 torqueAxis1 = -constraintNormal;
+		solverConstraint.m_relpos2CrossNormal = torqueAxis1;
+		solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);
+	}
+	else
+	{
+		btVector3 torqueAxis1 = -constraintNormal;
+		solverConstraint.m_relpos2CrossNormal = torqueAxis1;
+		solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);
+
+		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
+	}
 
-		if(!isFriction)
+	{
+		btScalar denom0 = 0.f;
+		btScalar denom1 = 0.f;
+		btScalar* jacB = 0;
+		btScalar* jacA = 0;
+		btScalar* lambdaA = 0;
+		btScalar* lambdaB = 0;
+		int ndofA = 0;
+		if (multiBodyA)
 		{
-			if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+			ndofA = multiBodyA->getNumDofs() + 6;
+			jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
+			lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+			for (int i = 0; i < ndofA; ++i)
 			{
-				//combine position and velocity into rhs
-				solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
-				solverConstraint.m_rhsPenetration = 0.f;
-
-			} else
+				btScalar j = jacA[i];
+				btScalar l = lambdaA[i];
+				denom0 += j * l;
+			}
+		}
+		else
+		{
+			if (rb0)
 			{
-				//split position and velocity into rhs and m_rhsPenetration
-				solverConstraint.m_rhs = velocityImpulse;
-				solverConstraint.m_rhsPenetration = penetrationImpulse;
+				btVector3 iMJaA = rb0 ? rb0->getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3(0, 0, 0);
+				denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
+			}
+		}
+		if (multiBodyB)
+		{
+			const int ndofB = multiBodyB->getNumDofs() + 6;
+			jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
+			lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+			for (int i = 0; i < ndofB; ++i)
+			{
+				btScalar j = jacB[i];
+				btScalar l = lambdaB[i];
+				denom1 += j * l;
 			}
+		}
+		else
+		{
+			if (rb1)
+			{
+				btVector3 iMJaB = rb1 ? rb1->getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3(0, 0, 0);
+				denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
+			}
+		}
 
-			solverConstraint.m_lowerLimit = 0;
-			solverConstraint.m_upperLimit = 1e10f;
+		btScalar d = denom0 + denom1 + infoGlobal.m_globalCfm;
+		if (d > SIMD_EPSILON)
+		{
+			solverConstraint.m_jacDiagABInv = relaxation / (d);
 		}
 		else
 		{
-			solverConstraint.m_rhs = velocityImpulse;
-			solverConstraint.m_rhsPenetration = 0.f;
-			solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
-			solverConstraint.m_upperLimit = solverConstraint.m_friction;
+			//disable the constraint row to handle singularity/redundant constraint
+			solverConstraint.m_jacDiagABInv = 0.f;
+		}
+	}
+
+	//compute rhs and remaining solverConstraint fields
+
+	btScalar restitution = 0.f;
+	btScalar penetration = isFriction ? 0 : cp.getDistance();
+
+	btScalar rel_vel = 0.f;
+	int ndofA = 0;
+	int ndofB = 0;
+	{
+		btVector3 vel1, vel2;
+		if (multiBodyA)
+		{
+			ndofA = multiBodyA->getNumDofs() + 6;
+			btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
+			for (int i = 0; i < ndofA; ++i)
+				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
+		}
+		else
+		{
+			if (rb0)
+			{
+				btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
+				rel_vel += solverConstraint.m_contactNormal1.dot(rb0 ? solverBodyA->m_linearVelocity + solverBodyA->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(rb0 ? solverBodyA->m_angularVelocity : btVector3(0, 0, 0));
+			}
+		}
+		if (multiBodyB)
+		{
+			ndofB = multiBodyB->getNumDofs() + 6;
+			btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
+			for (int i = 0; i < ndofB; ++i)
+				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
+		}
+		else
+		{
+			if (rb1)
+			{
+				btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
+				rel_vel += solverConstraint.m_contactNormal2.dot(rb1 ? solverBodyB->m_linearVelocity + solverBodyB->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(rb1 ? solverBodyB->m_angularVelocity : btVector3(0, 0, 0));
+			}
 		}
 
-		solverConstraint.m_cfm = 0.f;			//why not use cfmSlip?
+		solverConstraint.m_friction = combinedTorsionalFriction;
+
+		if (!isFriction)
+		{
+			restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
+			if (restitution <= btScalar(0.))
+			{
+				restitution = 0.f;
+			}
+		}
 	}
 
-}
+	solverConstraint.m_appliedImpulse = 0.f;
+	solverConstraint.m_appliedPushImpulse = 0.f;
 
+	{
+		btScalar velocityError = 0 - rel_vel;  // * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
 
+		btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
 
+		solverConstraint.m_rhs = velocityImpulse;
+		solverConstraint.m_rhsPenetration = 0.f;
+		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
+		solverConstraint.m_upperLimit = solverConstraint.m_friction;
 
-btMultiBodySolverConstraint&	btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
+		solverConstraint.m_cfm = infoGlobal.m_globalCfm * solverConstraint.m_jacDiagABInv;
+	}
+}
+
+btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
 {
 	BT_PROFILE("addMultiBodyFrictionConstraint");
 	btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
-    solverConstraint.m_orgConstraint = 0;
-    solverConstraint.m_orgDofIndex = -1;
-    
+	solverConstraint.m_orgConstraint = 0;
+	solverConstraint.m_orgDofIndex = -1;
+
 	solverConstraint.m_frictionIndex = frictionIndex;
 	bool isFriction = true;
 
 	const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
 	const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
-	
-	btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
-	btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
 
-	int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
-	int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
+	btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
+	btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
+
+	int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
+	int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
+
+	solverConstraint.m_solverBodyIdA = solverBodyIdA;
+	solverConstraint.m_solverBodyIdB = solverBodyIdB;
+	solverConstraint.m_multiBodyA = mbA;
+	if (mbA)
+		solverConstraint.m_linkA = fcA->m_link;
+
+	solverConstraint.m_multiBodyB = mbB;
+	if (mbB)
+		solverConstraint.m_linkB = fcB->m_link;
+
+	solverConstraint.m_originalContactPoint = &cp;
+
+	setupMultiBodyContactConstraint(solverConstraint, normalAxis, 0, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
+	return solverConstraint;
+}
+
+btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
+																								  btScalar combinedTorsionalFriction,
+																								  btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
+{
+	BT_PROFILE("addMultiBodyRollingFrictionConstraint");
+
+	bool useTorsionalAndConeFriction = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0));
+
+	btMultiBodySolverConstraint& solverConstraint = useTorsionalAndConeFriction ? m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing() : m_multiBodyFrictionContactConstraints.expandNonInitializing();
+	solverConstraint.m_orgConstraint = 0;
+	solverConstraint.m_orgDofIndex = -1;
+
+	solverConstraint.m_frictionIndex = frictionIndex;
+	bool isFriction = true;
+
+	const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
+	const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
+
+	btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
+	btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
+
+	int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
+	int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
 
 	solverConstraint.m_solverBodyIdA = solverBodyIdA;
 	solverConstraint.m_solverBodyIdB = solverBodyIdB;
@@ -561,54 +1210,89 @@ btMultiBodySolverConstraint&	btMultiBodyConstraintSolver::addMultiBodyFrictionCo
 
 	solverConstraint.m_originalContactPoint = &cp;
 
-	setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
+	setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
 
-void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
+btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodySpinningFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
+	btScalar combinedTorsionalFriction,
+	btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
+{
+	BT_PROFILE("addMultiBodyRollingFrictionConstraint");
+
+	btMultiBodySolverConstraint& solverConstraint = m_multiBodySpinningFrictionContactConstraints.expandNonInitializing();
+	solverConstraint.m_orgConstraint = 0;
+	solverConstraint.m_orgDofIndex = -1;
+
+	solverConstraint.m_frictionIndex = frictionIndex;
+	bool isFriction = true;
+
+	const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
+	const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
+
+	btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
+	btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
+
+	int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
+	int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
+
+	solverConstraint.m_solverBodyIdA = solverBodyIdA;
+	solverConstraint.m_solverBodyIdB = solverBodyIdB;
+	solverConstraint.m_multiBodyA = mbA;
+	if (mbA)
+		solverConstraint.m_linkA = fcA->m_link;
+
+	solverConstraint.m_multiBodyB = mbB;
+	if (mbB)
+		solverConstraint.m_linkB = fcB->m_link;
+
+	solverConstraint.m_originalContactPoint = &cp;
+
+	setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
+	return solverConstraint;
+}
+void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal)
 {
 	const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
 	const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
 	
-	btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
-	btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
+	btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
+	btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
 
-	btCollisionObject* colObj0=0,*colObj1=0;
+	btCollisionObject *colObj0 = 0, *colObj1 = 0;
 
 	colObj0 = (btCollisionObject*)manifold->getBody0();
 	colObj1 = (btCollisionObject*)manifold->getBody1();
 
-	int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
-	int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
-
-//	btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
-//	btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
+	int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
+	int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
 
+	//	btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
+	//	btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
 
 	///avoid collision response between two static objects
-//	if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
+	//	if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
 	//	return;
 
+	//only a single rollingFriction per manifold
+	int rollingFriction = 4;
 
-
-	for (int j=0;j<manifold->getNumContacts();j++)
+	for (int j = 0; j < manifold->getNumContacts(); j++)
 	{
-
 		btManifoldPoint& cp = manifold->getContactPoint(j);
 
 		if (cp.getDistance() <= manifold->getContactProcessingThreshold())
 		{
-		
 			btScalar relaxation;
 
 			int frictionIndex = m_multiBodyNormalContactConstraints.size();
 
 			btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
 
-	//		btRigidBody* rb0 = btRigidBody::upcast(colObj0);
-	//		btRigidBody* rb1 = btRigidBody::upcast(colObj1);
-            solverConstraint.m_orgConstraint = 0;
-            solverConstraint.m_orgDofIndex = -1;
+			//		btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+			//		btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+			solverConstraint.m_orgConstraint = 0;
+			solverConstraint.m_orgDofIndex = -1;
 			solverConstraint.m_solverBodyIdA = solverBodyIdA;
 			solverConstraint.m_solverBodyIdB = solverBodyIdB;
 			solverConstraint.m_multiBodyA = mbA;
@@ -622,71 +1306,56 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 			solverConstraint.m_originalContactPoint = &cp;
 
 			bool isFriction = false;
-			setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction);
+			setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp.m_appliedImpulse, cp, infoGlobal, relaxation, isFriction);
 
-//			const btVector3& pos1 = cp.getPositionWorldOnA();
-//			const btVector3& pos2 = cp.getPositionWorldOnB();
+			//			const btVector3& pos1 = cp.getPositionWorldOnA();
+			//			const btVector3& pos2 = cp.getPositionWorldOnB();
 
 			/////setup the friction constraints
 #define ENABLE_FRICTION
 #ifdef ENABLE_FRICTION
-			solverConstraint.m_frictionIndex = frictionIndex;
-#if ROLLING_FRICTION
-	int rollingFriction=1;
-			btVector3 angVelA(0,0,0),angVelB(0,0,0);
-			if (rb0)
-				angVelA = rb0->getAngularVelocity();
-			if (rb1)
-				angVelB = rb1->getAngularVelocity();
-			btVector3 relAngVel = angVelB-angVelA;
-
-			if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
-			{
-				//only a single rollingFriction per manifold
-				rollingFriction--;
-				if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
-				{
-					relAngVel.normalize();
-					applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					if (relAngVel.length()>0.001)
-						addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
-				} else
-				{
-					addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-					btVector3 axis0,axis1;
-					btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
-					applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					if (axis0.length()>0.001)
-						addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-					if (axis1.length()>0.001)
-						addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-		
-				}
-			}
-#endif //ROLLING_FRICTION
+			solverConstraint.m_frictionIndex = m_multiBodyFrictionContactConstraints.size();
 
 			///Bullet has several options to set the friction directions
-			///By default, each contact has only a single friction direction that is recomputed automatically very frame 
+			///By default, each contact has only a single friction direction that is recomputed automatically every frame
 			///based on the relative linear velocity.
-			///If the relative velocity it zero, it will automatically compute a friction direction.
-			
+			///If the relative velocity is zero, it will automatically compute a friction direction.
+
 			///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
 			///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
 			///
 			///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
 			///
-			///The user can manually override the friction directions for certain contacts using a contact callback, 
+			///The user can manually override the friction directions for certain contacts using a contact callback,
 			///and set the cp.m_lateralFrictionInitialized to true
 			///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
 			///this will give a conveyor belt effect
 			///
-			if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
-			{/*
+
+			btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2);
+			cp.m_lateralFrictionDir1.normalize();
+			cp.m_lateralFrictionDir2.normalize();
+
+			if (rollingFriction > 0)
+			{
+				if (cp.m_combinedSpinningFriction > 0)
+				{
+					addMultiBodySpinningFrictionConstraint(cp.m_normalWorldOnB, manifold, frictionIndex, cp, cp.m_combinedSpinningFriction, colObj0, colObj1, relaxation, infoGlobal);
+				}
+				if (cp.m_combinedRollingFriction > 0)
+				{
+					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+
+					addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
+					addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
+				}
+				rollingFriction--;
+			}
+			if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
+			{ /*
 				cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
 				btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
 				if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
@@ -709,83 +1378,123 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 				} else
 				*/
 				{
-					btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
-
-					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
+					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
-						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
+						applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+						applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
 					}
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
 					{
-						cp.m_lateralFrictionInitialized = true;
+						cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
 					}
 				}
-
-			} else
+			}
+			else
 			{
-				addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1);
+				addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
 
 				if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2);
-
-				//setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
-				//todo:
+					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
 				solverConstraint.m_appliedImpulse = 0.f;
 				solverConstraint.m_appliedPushImpulse = 0.f;
-			}
-		
-
-#endif //ENABLE_FRICTION
+      }
 
+#endif  //ENABLE_FRICTION
+		}
+		else
+		{
+			// Reset quantities related to warmstart as 0.
+			cp.m_appliedImpulse = 0;
+			cp.m_prevRHS = 0;
 		}
 	}
 }
 
-void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
+void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
 {
-	//btPersistentManifold* manifold = 0;
-
-	for (int i=0;i<numManifolds;i++)
+	for (int i = 0; i < numManifolds; i++)
 	{
-		btPersistentManifold* manifold= manifoldPtr[i];
+		btPersistentManifold* manifold = manifoldPtr[i];
 		const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
 		const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
 		if (!fcA && !fcB)
 		{
 			//the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
-			convertContact(manifold,infoGlobal);
-		} else
+			convertContact(manifold, infoGlobal);
+		}
+		else
 		{
-			convertMultiBodyContact(manifold,infoGlobal);
+			convertMultiBodyContact(manifold, infoGlobal);
 		}
 	}
 
 	//also convert the multibody constraints, if any
 
-	
-	for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
+	for (int i = 0; i < m_tmpNumMultiBodyConstraints; i++)
 	{
 		btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
 		m_data.m_solverBodyPool = &m_tmpSolverBodyPool;
 		m_data.m_fixedBodyId = m_fixedBodyId;
-		
-		c->createConstraintRows(m_multiBodyNonContactConstraints,m_data,	infoGlobal);
+
+		c->createConstraintRows(m_multiBodyNonContactConstraints, m_data, infoGlobal);
 	}
 
+	// Warmstart for noncontact constraints
+	if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING)
+	{
+		for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
+		{
+			btMultiBodySolverConstraint& solverConstraint =
+				m_multiBodyNonContactConstraints[i];
+			solverConstraint.m_appliedImpulse =
+				solverConstraint.m_orgConstraint->getAppliedImpulse(solverConstraint.m_orgDofIndex) *
+				infoGlobal.m_articulatedWarmstartingFactor;
+
+			btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
+			btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
+			if (solverConstraint.m_appliedImpulse)
+			{
+				if (multiBodyA)
+				{
+					int ndofA = multiBodyA->getNumDofs() + 6;
+					btScalar* deltaV =
+						&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+					btScalar impulse = solverConstraint.m_appliedImpulse;
+					multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
+					applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
+				}
+				if (multiBodyB)
+				{
+					int ndofB = multiBodyB->getNumDofs() + 6;
+					btScalar* deltaV =
+						&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+					btScalar impulse = solverConstraint.m_appliedImpulse;
+					multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
+					applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
+				}
+			}
+		}
+	}
+	else
+	{
+		for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
+		{
+			btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
+			solverConstraint.m_appliedImpulse = 0;
+		}
+	}
 }
 
-
-
-btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
+btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
 {
-	return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
+	//printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints);
+	return btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
 }
 
 #if 0
@@ -811,53 +1520,52 @@ static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodyS
 
 void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
 {
-#if 1 
-	
+#if 1
+
 	//bod->addBaseForce(m_gravity * bod->getBaseMass());
 	//bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
 
 	if (c.m_orgConstraint)
 	{
-		c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse);
+		c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex, c.m_appliedImpulse);
 	}
-	
 
 	if (c.m_multiBodyA)
 	{
-		
 		c.m_multiBodyA->setCompanionId(-1);
-		btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
-		btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
-		if (c.m_linkA<0)
+		btVector3 force = c.m_contactNormal1 * (c.m_appliedImpulse / deltaTime);
+		btVector3 torque = c.m_relpos1CrossNormal * (c.m_appliedImpulse / deltaTime);
+		if (c.m_linkA < 0)
 		{
 			c.m_multiBodyA->addBaseConstraintForce(force);
 			c.m_multiBodyA->addBaseConstraintTorque(torque);
-		} else
+		}
+		else
 		{
-			c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force);
-				//b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
-			c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque);
+			c.m_multiBodyA->addLinkConstraintForce(c.m_linkA, force);
+			//b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
+			c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA, torque);
 		}
 	}
-	
+
 	if (c.m_multiBodyB)
 	{
 		{
 			c.m_multiBodyB->setCompanionId(-1);
-			btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
-			btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
-			if (c.m_linkB<0)
+			btVector3 force = c.m_contactNormal2 * (c.m_appliedImpulse / deltaTime);
+			btVector3 torque = c.m_relpos2CrossNormal * (c.m_appliedImpulse / deltaTime);
+			if (c.m_linkB < 0)
 			{
 				c.m_multiBodyB->addBaseConstraintForce(force);
 				c.m_multiBodyB->addBaseConstraintTorque(torque);
-			} else
+			}
+			else
 			{
 				{
-					c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force);
+					c.m_multiBodyB->addLinkConstraintForce(c.m_linkB, force);
 					//b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
-					c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque);
+					c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB, torque);
 				}
-					
 			}
 		}
 	}
@@ -867,82 +1575,65 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
 
 	if (c.m_multiBodyA)
 	{
-		
-		if(c.m_multiBodyA->isMultiDof())
-		{
-			c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
-		}
-		else
-		{
-			c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
-		}
+		c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], c.m_appliedImpulse);
 	}
-	
+
 	if (c.m_multiBodyB)
 	{
-		if(c.m_multiBodyB->isMultiDof())
-		{
-			c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
-		}
-		else
-		{
-			c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
-		}
+		c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], c.m_appliedImpulse);
 	}
 #endif
-
-
-
 }
 
-btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
+btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
 {
 	BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
 	int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
-	
 
-	//write back the delta v to the multi bodies, either as applied impulse (direct velocity change) 
+	//write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
 	//or as applied force, so we can measure the joint reaction forces easier
-	for (int i=0;i<numPoolConstraints;i++)
+	for (int i = 0; i < numPoolConstraints; i++)
 	{
 		btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i];
-		writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
+		writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep);
 
-		writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep);
+		writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex], infoGlobal.m_timeStep);
 
 		if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 		{
-			writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep);
+			writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1], infoGlobal.m_timeStep);
 		}
 	}
 
-
-	for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
+	for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
 	{
 		btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
-		writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
+		writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep);
 	}
 
-	
-	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+
 	{
 		BT_PROFILE("warm starting write back");
-		for (int j=0;j<numPoolConstraints;j++)
+		for (int j = 0; j < numPoolConstraints; j++)
 		{
 			const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
-			btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint;
+			btManifoldPoint* pt = (btManifoldPoint*)solverConstraint.m_originalContactPoint;
 			btAssert(pt);
 			pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
+ 		  pt->m_prevRHS = solverConstraint.m_rhs;
 			pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
-			
+
 			//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
 			if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 			{
-				pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse;
+				pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1].m_appliedImpulse;
+			} else
+			{
+				pt->m_appliedImpulseLateral2 = 0;
 			}
-			//do a callback here?
 		}
 	}
+
 #if 0
 	//multibody joint feedback
 	{
@@ -1040,23 +1731,22 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
 		}
 
 	}
-#endif 
+#endif
 #endif
 
-	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
+	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
 }
 
-
-void  btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
+void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
 {
+	//printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints);
+
 	//printf("solveMultiBodyGroup start\n");
 	m_tmpMultiBodyConstraints = multiBodyConstraints;
 	m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
-	
-	btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
+
+	btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
 
 	m_tmpMultiBodyConstraints = 0;
 	m_tmpNumMultiBodyConstraints = 0;
-	
-
 }
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
index 321ee4231a3..f584360e2b6 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
@@ -25,63 +25,77 @@ class btMultiBody;
 
 #include "btMultiBodyConstraint.h"
 
-
-
-ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
+ATTRIBUTE_ALIGNED16(class)
+btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
 {
-
 protected:
+	btMultiBodyConstraintArray m_multiBodyNonContactConstraints;
 
-	btMultiBodyConstraintArray			m_multiBodyNonContactConstraints;
+	btMultiBodyConstraintArray m_multiBodyNormalContactConstraints;
+	btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints;
+	btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints;
+	btMultiBodyConstraintArray m_multiBodySpinningFrictionContactConstraints;
 
-	btMultiBodyConstraintArray			m_multiBodyNormalContactConstraints;
-	btMultiBodyConstraintArray			m_multiBodyFrictionContactConstraints;
+	btMultiBodyJacobianData m_data;
 
-	btMultiBodyJacobianData				m_data;
-	
 	//temp storage for multi body constraints for a specific island/group called by 'solveGroup'
-	btMultiBodyConstraint**					m_tmpMultiBodyConstraints;
-	int										m_tmpNumMultiBodyConstraints;
-
-	void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
-	
-
-	void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
-	btMultiBodySolverConstraint&	addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
-
-
-	void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, 
-																 btScalar* jacA,btScalar* jacB,
-																 btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
-																 const btContactSolverInfo& infoGlobal);
+	btMultiBodyConstraint** m_tmpMultiBodyConstraints;
+	int m_tmpNumMultiBodyConstraints;
+
+	btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
+
+	//solve 2 friction directions and clamp against the implicit friction cone
+	btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB);
+
+	void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
+
+	btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+
+	btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
+																		 btScalar combinedTorsionalFriction,
+																		 btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+
+	btMultiBodySolverConstraint& addMultiBodySpinningFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
+		btScalar combinedTorsionalFriction,
+		btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+
+	void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint & constraintRow,
+											btScalar * jacA, btScalar * jacB,
+											btScalar penetration, btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
+											const btContactSolverInfo& infoGlobal);
+
+	void setupMultiBodyContactConstraint(btMultiBodySolverConstraint & solverConstraint,
+										 const btVector3& contactNormal,
+                     const btScalar& appliedImpulse,
+										 btManifoldPoint& cp,
+                     const btContactSolverInfo& infoGlobal,
+										 btScalar& relaxation,
+										 bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+
+	//either rolling or spinning friction
+	void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint & solverConstraint,
+												   const btVector3& contactNormal,
+												   btManifoldPoint& cp,
+												   btScalar combinedTorsionalFriction,
+												   const btContactSolverInfo& infoGlobal,
+												   btScalar& relaxation,
+												   bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+
+	void convertMultiBodyContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal);
+	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+	//	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+	void applyDeltaVee(btScalar * deltaV, btScalar impulse, int velocityIndex, int ndof);
+	void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint & constraint, btScalar deltaTime);
 
-	void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, 
-																 const btVector3& contactNormal,
-																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
-																 btScalar& relaxation,
-																 bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
-
-	void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
-	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-//	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-
-	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-	void	applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof);
-	void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& constraint, btScalar deltaTime);
 public:
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
 	///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints)
-	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
-	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
-	
-	virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
-};
+	virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
+	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
 
-	
-	
-
-
-#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H
+	virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
+};
 
+#endif  //BT_MULTIBODY_CONSTRAINT_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
index a9c0b33b3a3..f599c9ccb6f 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
@@ -23,45 +23,49 @@ subject to the following restrictions:
 #include "LinearMath/btIDebugDraw.h"
 #include "LinearMath/btSerializer.h"
 
-
-void	btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)
+void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, int group, int mask)
 {
 	m_multiBodies.push_back(body);
-
 }
 
-void	btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
+void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
 {
 	m_multiBodies.remove(body);
 }
 
-void	btMultiBodyDynamicsWorld::calculateSimulationIslands()
+void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+    btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
+    predictMultiBodyTransforms(timeStep);
+    
+}
+void btMultiBodyDynamicsWorld::calculateSimulationIslands()
 {
 	BT_PROFILE("calculateSimulationIslands");
 
-	getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
+	getSimulationIslandManager()->updateActivationState(getCollisionWorld(), getCollisionWorld()->getDispatcher());
+
+	{
+		//merge islands based on speculative contact manifolds too
+		for (int i = 0; i < this->m_predictiveManifolds.size(); i++)
+		{
+			btPersistentManifold* manifold = m_predictiveManifolds[i];
+
+			const btCollisionObject* colObj0 = manifold->getBody0();
+			const btCollisionObject* colObj1 = manifold->getBody1();
+
+			if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
+				((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
+			{
+				getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
+			}
+		}
+	}
 
-    {
-        //merge islands based on speculative contact manifolds too
-        for (int i=0;i<this->m_predictiveManifolds.size();i++)
-        {
-            btPersistentManifold* manifold = m_predictiveManifolds[i];
-            
-            const btCollisionObject* colObj0 = manifold->getBody0();
-            const btCollisionObject* colObj1 = manifold->getBody1();
-            
-            if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
-                ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
-            {
-				getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
-            }
-        }
-    }
-    
 	{
 		int i;
 		int numConstraints = int(m_constraints.size());
-		for (i=0;i< numConstraints ; i++ )
+		for (i = 0; i < numConstraints; i++)
 		{
 			btTypedConstraint* constraint = m_constraints[i];
 			if (constraint->isEnabled())
@@ -72,23 +76,23 @@ void	btMultiBodyDynamicsWorld::calculateSimulationIslands()
 				if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
 					((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
 				{
-					getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
+					getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag());
 				}
 			}
 		}
 	}
 
 	//merge islands linked by Featherstone link colliders
-	for (int i=0;i<m_multiBodies.size();i++)
+	for (int i = 0; i < m_multiBodies.size(); i++)
 	{
 		btMultiBody* body = m_multiBodies[i];
 		{
 			btMultiBodyLinkCollider* prev = body->getBaseCollider();
 
-			for (int b=0;b<body->getNumLinks();b++)
+			for (int b = 0; b < body->getNumLinks(); b++)
 			{
 				btMultiBodyLinkCollider* cur = body->getLink(b).m_collider;
-				
+
 				if (((cur) && (!(cur)->isStaticOrKinematicObject())) &&
 					((prev) && (!(prev)->isStaticOrKinematicObject())))
 				{
@@ -98,36 +102,31 @@ void	btMultiBodyDynamicsWorld::calculateSimulationIslands()
 				}
 				if (cur && !cur->isStaticOrKinematicObject())
 					prev = cur;
-				
 			}
 		}
 	}
 
 	//merge islands linked by multibody constraints
 	{
-		for (int i=0;i<this->m_multiBodyConstraints.size();i++)
+		for (int i = 0; i < this->m_multiBodyConstraints.size(); i++)
 		{
 			btMultiBodyConstraint* c = m_multiBodyConstraints[i];
 			int tagA = c->getIslandIdA();
 			int tagB = c->getIslandIdB();
-			if (tagA>=0 && tagB>=0)
+			if (tagA >= 0 && tagB >= 0)
 				getSimulationIslandManager()->getUnionFind().unite(tagA, tagB);
 		}
 	}
 
 	//Store the island id in each body
 	getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
-
 }
 
-
-void	btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
+void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
 {
 	BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState");
 
-	
-	
-	for ( int i=0;i<m_multiBodies.size();i++)
+	for (int i = 0; i < m_multiBodies.size(); i++)
 	{
 		btMultiBody* body = m_multiBodies[i];
 		if (body)
@@ -138,659 +137,529 @@ void	btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
 				btMultiBodyLinkCollider* col = body->getBaseCollider();
 				if (col && col->getActivationState() == ACTIVE_TAG)
 				{
-					col->setActivationState( WANTS_DEACTIVATION);
+					col->setActivationState(WANTS_DEACTIVATION);
 					col->setDeactivationTime(0.f);
 				}
-				for (int b=0;b<body->getNumLinks();b++)
+				for (int b = 0; b < body->getNumLinks(); b++)
 				{
 					btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
 					if (col && col->getActivationState() == ACTIVE_TAG)
 					{
-						col->setActivationState( WANTS_DEACTIVATION);
+						col->setActivationState(WANTS_DEACTIVATION);
 						col->setDeactivationTime(0.f);
 					}
 				}
-			} else
+			}
+			else
 			{
 				btMultiBodyLinkCollider* col = body->getBaseCollider();
 				if (col && col->getActivationState() != DISABLE_DEACTIVATION)
-					col->setActivationState( ACTIVE_TAG );
+					col->setActivationState(ACTIVE_TAG);
 
-				for (int b=0;b<body->getNumLinks();b++)
+				for (int b = 0; b < body->getNumLinks(); b++)
 				{
 					btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
 					if (col && col->getActivationState() != DISABLE_DEACTIVATION)
-						col->setActivationState( ACTIVE_TAG );
+						col->setActivationState(ACTIVE_TAG);
 				}
 			}
-
 		}
 	}
 
 	btDiscreteDynamicsWorld::updateActivationState(timeStep);
 }
 
-
-SIMD_FORCE_INLINE	int	btGetConstraintIslandId2(const btTypedConstraint* lhs)
+void btMultiBodyDynamicsWorld::getAnalyticsData(btAlignedObjectArray<btSolverAnalyticsData>& islandAnalyticsData) const
 {
-	int islandId;
-	
-	const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
-	const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
-	islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
-	return islandId;
-
+	islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData;
 }
 
-
-class btSortConstraintOnIslandPredicate2
+btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
+	: btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
+	  m_multiBodyConstraintSolver(constraintSolver)
 {
-	public:
-
-		bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const
-		{
-			int rIslandId0,lIslandId0;
-			rIslandId0 = btGetConstraintIslandId2(rhs);
-			lIslandId0 = btGetConstraintIslandId2(lhs);
-			return lIslandId0 < rIslandId0;
-		}
-};
-
-
+	//split impulse is not yet supported for Featherstone hierarchies
+	//	getSolverInfo().m_splitImpulse = false;
+	getSolverInfo().m_solverMode |= SOLVER_USE_2_FRICTION_DIRECTIONS;
+	m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
+}
 
-SIMD_FORCE_INLINE	int	btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs)
+btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld()
 {
-	int islandId;
-	
-	int islandTagA = lhs->getIslandIdA();
-	int islandTagB = lhs->getIslandIdB();
-	islandId= islandTagA>=0?islandTagA:islandTagB;
-	return islandId;
-
+	delete m_solverMultiBodyIslandCallback;
 }
 
-
-class btSortMultiBodyConstraintOnIslandPredicate
+void btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
 {
-	public:
-
-		bool operator() ( const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs ) const
-		{
-			int rIslandId0,lIslandId0;
-			rIslandId0 = btGetMultiBodyConstraintIslandId(rhs);
-			lIslandId0 = btGetMultiBodyConstraintIslandId(lhs);
-			return lIslandId0 < rIslandId0;
-		}
-};
+	m_multiBodyConstraintSolver = solver;
+	m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver);
+	btDiscreteDynamicsWorld::setConstraintSolver(solver);
+}
 
-struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
+void btMultiBodyDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 {
-	btContactSolverInfo*	m_solverInfo;
-	btMultiBodyConstraintSolver*		m_solver;
-	btMultiBodyConstraint**		m_multiBodySortedConstraints;
-	int							m_numMultiBodyConstraints;
-	
-	btTypedConstraint**		m_sortedConstraints;
-	int						m_numConstraints;
-	btIDebugDraw*			m_debugDrawer;
-	btDispatcher*			m_dispatcher;
-	
-	btAlignedObjectArray<btCollisionObject*> m_bodies;
-	btAlignedObjectArray<btPersistentManifold*> m_manifolds;
-	btAlignedObjectArray<btTypedConstraint*> m_constraints;
-	btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
-
-
-	MultiBodyInplaceSolverIslandCallback(	btMultiBodyConstraintSolver*	solver,
-									btDispatcher* dispatcher)
-		:m_solverInfo(NULL),
-		m_solver(solver),
-		m_multiBodySortedConstraints(NULL),
-		m_numConstraints(0),
-		m_debugDrawer(NULL),
-		m_dispatcher(dispatcher)
+	if (solver->getSolverType() == BT_MULTIBODY_SOLVER)
 	{
-
-	}
-
-	MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other)
-	{
-		btAssert(0);
-		(void)other;
-		return *this;
-	}
-
-	SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints,	int	numMultiBodyConstraints,	btIDebugDraw* debugDrawer)
-	{
-		btAssert(solverInfo);
-		m_solverInfo = solverInfo;
-
-		m_multiBodySortedConstraints = sortedMultiBodyConstraints;
-		m_numMultiBodyConstraints = numMultiBodyConstraints;
-		m_sortedConstraints = sortedConstraints;
-		m_numConstraints = numConstraints;
-
-		m_debugDrawer = debugDrawer;
-		m_bodies.resize (0);
-		m_manifolds.resize (0);
-		m_constraints.resize (0);
-		m_multiBodyConstraints.resize(0);
+		m_multiBodyConstraintSolver = (btMultiBodyConstraintSolver*)solver;
 	}
-
-	
-	virtual	void	processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
-	{
-		if (islandId<0)
-		{
-			///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
-			m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
-		} else
-		{
-				//also add all non-contact constraints/joints for this island
-			btTypedConstraint** startConstraint = 0;
-			btMultiBodyConstraint** startMultiBodyConstraint = 0;
-
-			int numCurConstraints = 0;
-			int numCurMultiBodyConstraints = 0;
-
-			int i;
-			
-			//find the first constraint for this island
-
-			for (i=0;i<m_numConstraints;i++)
-			{
-				if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
-				{
-					startConstraint = &m_sortedConstraints[i];
-					break;
-				}
-			}
-			//count the number of constraints in this island
-			for (;i<m_numConstraints;i++)
-			{
-				if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
-				{
-					numCurConstraints++;
-				}
-			}
-
-			for (i=0;i<m_numMultiBodyConstraints;i++)
-			{
-				if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
-				{
-					
-					startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
-					break;
-				}
-			}
-			//count the number of multi body constraints in this island
-			for (;i<m_numMultiBodyConstraints;i++)
-			{
-				if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
-				{
-					numCurMultiBodyConstraints++;
-				}
-			}
-
-			if (m_solverInfo->m_minimumSolverBatchSize<=1)
-			{
-				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
-			} else
-			{
-				
-				for (i=0;i<numBodies;i++)
-					m_bodies.push_back(bodies[i]);
-				for (i=0;i<numManifolds;i++)
-					m_manifolds.push_back(manifolds[i]);
-				for (i=0;i<numCurConstraints;i++)
-					m_constraints.push_back(startConstraint[i]);
-				
-				for (i=0;i<numCurMultiBodyConstraints;i++)
-					m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
-				
-				if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
-				{
-					processConstraints();
-				} else
-				{
-					//printf("deferred\n");
-				}
-			}
-		}
-	}
-	void	processConstraints()
-	{
-
-		btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
-		btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
-		btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
-		btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;			
-
-		//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
-	
-		m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher);
-		m_bodies.resize(0);
-		m_manifolds.resize(0);
-		m_constraints.resize(0);
-		m_multiBodyConstraints.resize(0);
-	}
-
-};
-
-
-
-btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
-	:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
-	m_multiBodyConstraintSolver(constraintSolver)
-{
-	//split impulse is not yet supported for Featherstone hierarchies
-	getSolverInfo().m_splitImpulse = false;
-	getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS;
-	m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher);
-}
-
-btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
-{
-	delete m_solverMultiBodyIslandCallback;
+	btDiscreteDynamicsWorld::setConstraintSolver(solver);
 }
 
-void	btMultiBodyDynamicsWorld::forwardKinematics()
+void btMultiBodyDynamicsWorld::forwardKinematics()
 {
-	btAlignedObjectArray<btQuaternion> world_to_local;
-	btAlignedObjectArray<btVector3> local_origin;
-
-	for (int b=0;b<m_multiBodies.size();b++)
+	for (int b = 0; b < m_multiBodies.size(); b++)
 	{
 		btMultiBody* bod = m_multiBodies[b];
-		bod->forwardKinematics(world_to_local,local_origin);
+		bod->forwardKinematics(m_scratch_world_to_local, m_scratch_local_origin);
 	}
 }
-void	btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
+void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
-	forwardKinematics();
-
-	btAlignedObjectArray<btScalar> scratch_r;
-	btAlignedObjectArray<btVector3> scratch_v;
-	btAlignedObjectArray<btMatrix3x3> scratch_m;
-
-
-	BT_PROFILE("solveConstraints");
-	
-	m_sortedConstraints.resize( m_constraints.size());
-	int i; 
-	for (i=0;i<getNumConstraints();i++)
-	{
-		m_sortedConstraints[i] = m_constraints[i];
-	}
-	m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
-	btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
-
-	m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
-	for (i=0;i<m_multiBodyConstraints.size();i++)
-	{
-		m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
-	}
-	m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
+    solveExternalForces(solverInfo);
+    buildIslands();
+    solveInternalConstraints(solverInfo);
+}
 
-	btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ?  &m_sortedMultiBodyConstraints[0] : 0;
-	
+void btMultiBodyDynamicsWorld::buildIslands()
+{
+    m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
+}
 
-	m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
-	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
-	
+void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo)
+{
 	/// solve all the constraints for this island
-	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
+	m_solverMultiBodyIslandCallback->processConstraints();
+	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
+    {
+        BT_PROFILE("btMultiBody stepVelocities");
+        for (int i = 0; i < this->m_multiBodies.size(); i++)
+        {
+            btMultiBody* bod = m_multiBodies[i];
+            
+            bool isSleeping = false;
+            
+            if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+            {
+                isSleeping = true;
+            }
+            for (int b = 0; b < bod->getNumLinks(); b++)
+            {
+                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                    isSleeping = true;
+            }
+            
+            if (!isSleeping)
+            {
+                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+                m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
+                m_scratch_v.resize(bod->getNumLinks() + 1);
+                m_scratch_m.resize(bod->getNumLinks() + 1);
+                
+                if (bod->internalNeedsJointFeedback())
+                {
+                    if (!bod->isUsingRK4Integration())
+                    {
+                        if (bod->internalNeedsJointFeedback())
+                        {
+                            bool isConstraintPass = true;
+                            bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
+                                                                                      getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                      getSolverInfo().m_jointFeedbackInJointFrame);
+                        }
+                    }
+                }
+            }
+        }
+    }
+    for (int i = 0; i < this->m_multiBodies.size(); i++)
+    {
+        btMultiBody* bod = m_multiBodies[i];
+        bod->processDeltaVeeMultiDof2();
+    }
+}
 
+void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo)
+{
+    forwardKinematics();
+    
+    BT_PROFILE("solveConstraints");
+    
+    clearMultiBodyConstraintForces();
+    
+    m_sortedConstraints.resize(m_constraints.size());
+    int i;
+    for (i = 0; i < getNumConstraints(); i++)
+    {
+        m_sortedConstraints[i] = m_constraints[i];
+    }
+    m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
+    btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
+    
+    m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
+    for (i = 0; i < m_multiBodyConstraints.size(); i++)
+    {
+        m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
+    }
+    m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
+    
+    btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
+    
+    m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
+    m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
+    
 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-	{
-		BT_PROFILE("btMultiBody addForce");
-		for (int i=0;i<this->m_multiBodies.size();i++)
-		{
-			btMultiBody* bod = m_multiBodies[i];
-
-			bool isSleeping = false;
-			
-			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
-			{
-				isSleeping = true;
-			} 
-			for (int b=0;b<bod->getNumLinks();b++)
-			{
-				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
-					isSleeping = true;
-			} 
-
-			if (!isSleeping)
-			{
-				//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
-				scratch_r.resize(bod->getNumLinks()+1);			//multidof? ("Y"s use it and it is used to store qdd)
-				scratch_v.resize(bod->getNumLinks()+1);
-				scratch_m.resize(bod->getNumLinks()+1);
-
-				bod->addBaseForce(m_gravity * bod->getBaseMass());
-
-				for (int j = 0; j < bod->getNumLinks(); ++j) 
-				{
-					bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
-				}
-			}//if (!isSleeping)
-		}
-	}
-#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-	
-
-	{
-		BT_PROFILE("btMultiBody stepVelocities");
-		for (int i=0;i<this->m_multiBodies.size();i++)
-		{
-			btMultiBody* bod = m_multiBodies[i];
-
-			bool isSleeping = false;
-			
-			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
-			{
-				isSleeping = true;
-			} 
-			for (int b=0;b<bod->getNumLinks();b++)
-			{
-				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
-					isSleeping = true;
-			} 
-
-			if (!isSleeping)
-			{
-				//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
-				scratch_r.resize(bod->getNumLinks()+1);			//multidof? ("Y"s use it and it is used to store qdd)
-				scratch_v.resize(bod->getNumLinks()+1);
-				scratch_m.resize(bod->getNumLinks()+1);
-				bool doNotUpdatePos = false;
-
-				{
-					if(!bod->isUsingRK4Integration())
-					{
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
-					}
-					else
-					{						
-						//
-						int numDofs = bod->getNumDofs() + 6;
-						int numPosVars = bod->getNumPosVars() + 7;
-						btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
-						//convenience
-						btScalar *pMem = &scratch_r2[0];
-						btScalar *scratch_q0 = pMem; pMem += numPosVars;
-						btScalar *scratch_qx = pMem; pMem += numPosVars;
-						btScalar *scratch_qd0 = pMem; pMem += numDofs;
-						btScalar *scratch_qd1 = pMem; pMem += numDofs;
-						btScalar *scratch_qd2 = pMem; pMem += numDofs;
-						btScalar *scratch_qd3 = pMem; pMem += numDofs;
-						btScalar *scratch_qdd0 = pMem; pMem += numDofs;
-						btScalar *scratch_qdd1 = pMem; pMem += numDofs;
-						btScalar *scratch_qdd2 = pMem; pMem += numDofs;
-						btScalar *scratch_qdd3 = pMem; pMem += numDofs;
-						btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
-
-						/////						
-						//copy q0 to scratch_q0 and qd0 to scratch_qd0
-						scratch_q0[0] = bod->getWorldToBaseRot().x();
-						scratch_q0[1] = bod->getWorldToBaseRot().y();
-						scratch_q0[2] = bod->getWorldToBaseRot().z();
-						scratch_q0[3] = bod->getWorldToBaseRot().w();
-						scratch_q0[4] = bod->getBasePos().x();
-						scratch_q0[5] = bod->getBasePos().y();
-						scratch_q0[6] = bod->getBasePos().z();
-						//
-						for(int link = 0; link < bod->getNumLinks(); ++link)
-						{
-							for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
-								scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];							
-						}
-						//
-						for(int dof = 0; dof < numDofs; ++dof)								
-							scratch_qd0[dof] = bod->getVelocityVector()[dof];
-						////
-						struct
-						{
-						    btMultiBody *bod;
+    {
+        BT_PROFILE("btMultiBody addForce");
+        for (int i = 0; i < this->m_multiBodies.size(); i++)
+        {
+            btMultiBody* bod = m_multiBodies[i];
+            
+            bool isSleeping = false;
+            
+            if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+            {
+                isSleeping = true;
+            }
+            for (int b = 0; b < bod->getNumLinks(); b++)
+            {
+                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                    isSleeping = true;
+            }
+            
+            if (!isSleeping)
+            {
+                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+                m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
+                m_scratch_v.resize(bod->getNumLinks() + 1);
+                m_scratch_m.resize(bod->getNumLinks() + 1);
+                
+                bod->addBaseForce(m_gravity * bod->getBaseMass());
+                
+                for (int j = 0; j < bod->getNumLinks(); ++j)
+                {
+                    bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
+                }
+            }  //if (!isSleeping)
+        }
+    }
+#endif  //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+    
+    {
+        BT_PROFILE("btMultiBody stepVelocities");
+        for (int i = 0; i < this->m_multiBodies.size(); i++)
+        {
+            btMultiBody* bod = m_multiBodies[i];
+            
+            bool isSleeping = false;
+            
+            if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+            {
+                isSleeping = true;
+            }
+            for (int b = 0; b < bod->getNumLinks(); b++)
+            {
+                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                    isSleeping = true;
+            }
+            
+            if (!isSleeping)
+            {
+                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+                m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
+                m_scratch_v.resize(bod->getNumLinks() + 1);
+                m_scratch_m.resize(bod->getNumLinks() + 1);
+                bool doNotUpdatePos = false;
+                bool isConstraintPass = false;
+                {
+                    if (!bod->isUsingRK4Integration())
+                    {
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
+                                                                                  m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
+                                                                                  getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                    }
+                    else
+                    {
+                        //
+                        int numDofs = bod->getNumDofs() + 6;
+                        int numPosVars = bod->getNumPosVars() + 7;
+                        btAlignedObjectArray<btScalar> scratch_r2;
+                        scratch_r2.resize(2 * numPosVars + 8 * numDofs);
+                        //convenience
+                        btScalar* pMem = &scratch_r2[0];
+                        btScalar* scratch_q0 = pMem;
+                        pMem += numPosVars;
+                        btScalar* scratch_qx = pMem;
+                        pMem += numPosVars;
+                        btScalar* scratch_qd0 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qd1 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qd2 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qd3 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd0 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd1 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd2 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd3 = pMem;
+                        pMem += numDofs;
+                        btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]);
+                        
+                        /////
+                        //copy q0 to scratch_q0 and qd0 to scratch_qd0
+                        scratch_q0[0] = bod->getWorldToBaseRot().x();
+                        scratch_q0[1] = bod->getWorldToBaseRot().y();
+                        scratch_q0[2] = bod->getWorldToBaseRot().z();
+                        scratch_q0[3] = bod->getWorldToBaseRot().w();
+                        scratch_q0[4] = bod->getBasePos().x();
+                        scratch_q0[5] = bod->getBasePos().y();
+                        scratch_q0[6] = bod->getBasePos().z();
+                        //
+                        for (int link = 0; link < bod->getNumLinks(); ++link)
+                        {
+                            for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
+                                scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
+                        }
+                        //
+                        for (int dof = 0; dof < numDofs; ++dof)
+                            scratch_qd0[dof] = bod->getVelocityVector()[dof];
+                        ////
+                        struct
+                        {
+                            btMultiBody* bod;
                             btScalar *scratch_qx, *scratch_q0;
-
-						    void operator()()
-						    {
-						        for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
+                            
+                            void operator()()
+                            {
+                                for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
                                     scratch_qx[dof] = scratch_q0[dof];
-						    }
-						} pResetQx = {bod, scratch_qx, scratch_q0};
-						//
-						struct
-						{
-						    void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
-						    {
-						        for(int i = 0; i < size; ++i)
+                            }
+                        } pResetQx = {bod, scratch_qx, scratch_q0};
+                        //
+                        struct
+                        {
+                            void operator()(btScalar dt, const btScalar* pDer, const btScalar* pCurVal, btScalar* pVal, int size)
+                            {
+                                for (int i = 0; i < size; ++i)
                                     pVal[i] = pCurVal[i] + dt * pDer[i];
-						    }
-
-						} pEulerIntegrate;
-						//
-						struct
+                            }
+                            
+                        } pEulerIntegrate;
+                        //
+                        struct
                         {
-                            void operator()(btMultiBody *pBody, const btScalar *pData)
+                            void operator()(btMultiBody* pBody, const btScalar* pData)
                             {
-                                btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
-
-                                for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
+                                btScalar* pVel = const_cast<btScalar*>(pBody->getVelocityVector());
+                                
+                                for (int i = 0; i < pBody->getNumDofs() + 6; ++i)
                                     pVel[i] = pData[i];
-
                             }
                         } pCopyToVelocityVector;
-						//
+                        //
                         struct
-						{
-						    void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
-						    {
-						        for(int i = 0; i < size; ++i)
+                        {
+                            void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size)
+                            {
+                                for (int i = 0; i < size; ++i)
                                     pDst[i] = pSrc[start + i];
-						    }
-						} pCopy;
-						//
-
-						btScalar h = solverInfo.m_timeStep;
-						#define output &scratch_r[bod->getNumDofs()]
-						//calc qdd0 from: q0 & qd0	
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
-						pCopy(output, scratch_qdd0, 0, numDofs);
-						//calc q1 = q0 + h/2 * qd0
-						pResetQx();
-						bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
-						//calc qd1 = qd0 + h/2 * qdd0
-						pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
-						//
-						//calc qdd1 from: q1 & qd1
-						pCopyToVelocityVector(bod, scratch_qd1);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
-						pCopy(output, scratch_qdd1, 0, numDofs);
-						//calc q2 = q0 + h/2 * qd1
-						pResetQx();
-						bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
-						//calc qd2 = qd0 + h/2 * qdd1
-						pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
-						//
-						//calc qdd2 from: q2 & qd2
-						pCopyToVelocityVector(bod, scratch_qd2);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
-						pCopy(output, scratch_qdd2, 0, numDofs);
-						//calc q3 = q0 + h * qd2
-						pResetQx();
-						bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
-						//calc qd3 = qd0 + h * qdd2
-						pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
-						//
-						//calc qdd3 from: q3 & qd3
-						pCopyToVelocityVector(bod, scratch_qd3);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
-						pCopy(output, scratch_qdd3, 0, numDofs);
-
-						//
-						//calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
-						//calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)						
-						btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
-						btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
-						for(int i = 0; i < numDofs; ++i)
-						{
-							delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
-							delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);							
-							//delta_q[i] = h*scratch_qd0[i];
-							//delta_qd[i] = h*scratch_qdd0[i];
-						}
-						//
-						pCopyToVelocityVector(bod, scratch_qd0);
-						bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);						
-						//
-						if(!doNotUpdatePos)
-						{
-							btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
-							pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
-
-							for(int i = 0; i < numDofs; ++i)
-								pRealBuf[i] = delta_q[i];
-
-							//bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
-							bod->setPosUpdated(true);							
-						}
-
-						//ugly hack which resets the cached data to t0 (needed for constraint solver)
-						{
-							for(int link = 0; link < bod->getNumLinks(); ++link)
-								bod->getLink(link).updateCacheMultiDof();
-							bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, scratch_r, scratch_v, scratch_m);
-						}
-						
-					}
-				}
-				
-#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-				bod->clearForcesAndTorques();
-#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-			}//if (!isSleeping)
-		}
-	}
-
-	clearMultiBodyConstraintForces();
-
-	m_solverMultiBodyIslandCallback->processConstraints();
-	
-	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
-
-	{
-                BT_PROFILE("btMultiBody stepVelocities");
-                for (int i=0;i<this->m_multiBodies.size();i++)
-                {
-                        btMultiBody* bod = m_multiBodies[i];
-
-                        bool isSleeping = false;
-
-                        if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+                            }
+                        } pCopy;
+                        //
+                        
+                        btScalar h = solverInfo.m_timeStep;
+#define output &m_scratch_r[bod->getNumDofs()]
+                        //calc qdd0 from: q0 & qd0
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd0, 0, numDofs);
+                        //calc q1 = q0 + h/2 * qd0
+                        pResetQx();
+                        bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0);
+                        //calc qd1 = qd0 + h/2 * qdd0
+                        pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
+                        //
+                        //calc qdd1 from: q1 & qd1
+                        pCopyToVelocityVector(bod, scratch_qd1);
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd1, 0, numDofs);
+                        //calc q2 = q0 + h/2 * qd1
+                        pResetQx();
+                        bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1);
+                        //calc qd2 = qd0 + h/2 * qdd1
+                        pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
+                        //
+                        //calc qdd2 from: q2 & qd2
+                        pCopyToVelocityVector(bod, scratch_qd2);
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd2, 0, numDofs);
+                        //calc q3 = q0 + h * qd2
+                        pResetQx();
+                        bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
+                        //calc qd3 = qd0 + h * qdd2
+                        pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
+                        //
+                        //calc qdd3 from: q3 & qd3
+                        pCopyToVelocityVector(bod, scratch_qd3);
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd3, 0, numDofs);
+                        
+                        //
+                        //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
+                        //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
+                        btAlignedObjectArray<btScalar> delta_q;
+                        delta_q.resize(numDofs);
+                        btAlignedObjectArray<btScalar> delta_qd;
+                        delta_qd.resize(numDofs);
+                        for (int i = 0; i < numDofs; ++i)
                         {
-                                isSleeping = true;
+                            delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]);
+                            delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]);
+                            //delta_q[i] = h*scratch_qd0[i];
+                            //delta_qd[i] = h*scratch_qdd0[i];
                         }
-                        for (int b=0;b<bod->getNumLinks();b++)
+                        //
+                        pCopyToVelocityVector(bod, scratch_qd0);
+                        bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
+                        //
+                        if (!doNotUpdatePos)
                         {
-                                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
-                                        isSleeping = true;
+                            btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
+                            pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
+                            
+                            for (int i = 0; i < numDofs; ++i)
+                                pRealBuf[i] = delta_q[i];
+                            
+                            //bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
+                            bod->setPosUpdated(true);
                         }
-
-                        if (!isSleeping)
+                        
+                        //ugly hack which resets the cached data to t0 (needed for constraint solver)
                         {
-                                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
-                                scratch_r.resize(bod->getNumLinks()+1);                 //multidof? ("Y"s use it and it is used to store qdd)
-                                scratch_v.resize(bod->getNumLinks()+1);
-                                scratch_m.resize(bod->getNumLinks()+1);
-
-                                
-                            {
-                                if(!bod->isUsingRK4Integration())
-                                {
-									bool isConstraintPass = true;
-                                    bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass);
-                                }
-				}
-			}
-		}
-	}
-
-	for (int i=0;i<this->m_multiBodies.size();i++)
-	{
-		btMultiBody* bod = m_multiBodies[i];
-		bod->processDeltaVeeMultiDof2();
-	}
-
+                            for (int link = 0; link < bod->getNumLinks(); ++link)
+                                bod->getLink(link).updateCacheMultiDof();
+                            bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                      isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                      getSolverInfo().m_jointFeedbackInJointFrame);
+                        }
+                    }
+                }
+                
+#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+                bod->clearForcesAndTorques();
+#endif         //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+            }  //if (!isSleeping)
+        }
+    }
 }
 
-void	btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
+
+void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
 	btDiscreteDynamicsWorld::integrateTransforms(timeStep);
+    integrateMultiBodyTransforms(timeStep);
+}
 
-	{
+void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
+{
 		BT_PROFILE("btMultiBody stepPositions");
 		//integrate and update the Featherstone hierarchies
-		btAlignedObjectArray<btQuaternion> world_to_local;
-		btAlignedObjectArray<btVector3> local_origin;
 
-		for (int b=0;b<m_multiBodies.size();b++)
+		for (int b = 0; b < m_multiBodies.size(); b++)
 		{
 			btMultiBody* bod = m_multiBodies[b];
 			bool isSleeping = false;
 			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
 			{
 				isSleeping = true;
-			} 
-			for (int b=0;b<bod->getNumLinks();b++)
+			}
+			for (int b = 0; b < bod->getNumLinks(); b++)
 			{
-				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
+				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
 					isSleeping = true;
 			}
 
-
 			if (!isSleeping)
 			{
+				bod->addSplitV();
 				int nLinks = bod->getNumLinks();
 
 				///base + num m_links
-			
-				
-				{
-					if(!bod->isPosUpdated())
-						bod->stepPositionsMultiDof(timeStep);
-					else
-					{
-						btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
-						pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
+                if (!bod->isPosUpdated())
+                    bod->stepPositionsMultiDof(timeStep);
+                else
+                {
+                    btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
+                    pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
+
+                    bod->stepPositionsMultiDof(1, 0, pRealBuf);
+                    bod->setPosUpdated(false);
+                }
 
-						bod->stepPositionsMultiDof(1, 0, pRealBuf);
-						bod->setPosUpdated(false);
-					}
-				}
-				
-				world_to_local.resize(nLinks+1);
-				local_origin.resize(nLinks+1);
 
-				bod->updateCollisionObjectWorldTransforms(world_to_local,local_origin);
-				
-			} else
+				m_scratch_world_to_local.resize(nLinks + 1);
+				m_scratch_local_origin.resize(nLinks + 1);
+                bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
+				bod->substractSplitV();
+			}
+			else
 			{
 				bod->clearVelocities();
 			}
 		}
-	}
 }
 
+void btMultiBodyDynamicsWorld::predictMultiBodyTransforms(btScalar timeStep)
+{
+    BT_PROFILE("btMultiBody stepPositions");
+    //integrate and update the Featherstone hierarchies
+    
+    for (int b = 0; b < m_multiBodies.size(); b++)
+    {
+        btMultiBody* bod = m_multiBodies[b];
+        bool isSleeping = false;
+        if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+        {
+            isSleeping = true;
+        }
+        for (int b = 0; b < bod->getNumLinks(); b++)
+        {
+            if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                isSleeping = true;
+        }
+        
+        if (!isSleeping)
+        {
+            int nLinks = bod->getNumLinks();
+            bod->predictPositionsMultiDof(timeStep);
+            m_scratch_world_to_local.resize(nLinks + 1);
+            m_scratch_local_origin.resize(nLinks + 1);
+            bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
+        }
+        else
+        {
+            bod->clearVelocities();
+        }
+    }
+}
 
-
-void	btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)
+void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint)
 {
 	m_multiBodyConstraints.push_back(constraint);
 }
 
-void	btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)
+void btMultiBodyDynamicsWorld::removeMultiBodyConstraint(btMultiBodyConstraint* constraint)
 {
 	m_multiBodyConstraints.remove(constraint);
 }
@@ -800,16 +669,17 @@ void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstrain
 	constraint->debugDraw(getDebugDrawer());
 }
 
-
-void	btMultiBodyDynamicsWorld::debugDrawWorld()
+void btMultiBodyDynamicsWorld::debugDrawWorld()
 {
 	BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
 
+	btDiscreteDynamicsWorld::debugDrawWorld();
+
 	bool drawConstraints = false;
 	if (getDebugDrawer())
 	{
 		int mode = getDebugDrawer()->getDebugMode();
-		if (mode  & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
+		if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
 		{
 			drawConstraints = true;
 		}
@@ -817,159 +687,148 @@ void	btMultiBodyDynamicsWorld::debugDrawWorld()
 		if (drawConstraints)
 		{
 			BT_PROFILE("btMultiBody debugDrawWorld");
-			
-			btAlignedObjectArray<btQuaternion> world_to_local1;
-			btAlignedObjectArray<btVector3> local_origin1;
 
-			for (int c=0;c<m_multiBodyConstraints.size();c++)
+			for (int c = 0; c < m_multiBodyConstraints.size(); c++)
 			{
 				btMultiBodyConstraint* constraint = m_multiBodyConstraints[c];
 				debugDrawMultiBodyConstraint(constraint);
 			}
 
-			for (int b = 0; b<m_multiBodies.size(); b++)
+			for (int b = 0; b < m_multiBodies.size(); b++)
 			{
 				btMultiBody* bod = m_multiBodies[b];
-				bod->forwardKinematics(world_to_local1,local_origin1);
-				
-				getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
+				bod->forwardKinematics(m_scratch_world_to_local1, m_scratch_local_origin1);
 
+				if (mode & btIDebugDraw::DBG_DrawFrames)
+				{
+					getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
+				}
 
-				for (int m = 0; m<bod->getNumLinks(); m++)
+				for (int m = 0; m < bod->getNumLinks(); m++)
 				{
-					
 					const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
-
-					getDebugDrawer()->drawTransform(tr, 0.1);
-
-						//draw the joint axis
-					if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
+					if (mode & btIDebugDraw::DBG_DrawFrames)
 					{
-						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
-					
-						btVector4 color(0,0,0,1);//1,1,1);
-						btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
-						btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
-						getDebugDrawer()->drawLine(from,to,color);
+						getDebugDrawer()->drawTransform(tr, 0.1);
 					}
-					if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
+					//draw the joint axis
+					if (bod->getLink(m).m_jointType == btMultibodyLink::eRevolute)
 					{
-						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
-					
-						btVector4 color(0,0,0,1);//1,1,1);
-						btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
-						btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
-						getDebugDrawer()->drawLine(from,to,color);
+						btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_topVec) * 0.1;
+
+						btVector4 color(0, 0, 0, 1);  //1,1,1);
+						btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						getDebugDrawer()->drawLine(from, to, color);
 					}
-					if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
+					if (bod->getLink(m).m_jointType == btMultibodyLink::eFixed)
 					{
-						btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
-					
-						btVector4 color(0,0,0,1);//1,1,1);
-						btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
-						btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
-						getDebugDrawer()->drawLine(from,to,color);
+						btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_bottomVec) * 0.1;
+
+						btVector4 color(0, 0, 0, 1);  //1,1,1);
+						btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						getDebugDrawer()->drawLine(from, to, color);
+					}
+					if (bod->getLink(m).m_jointType == btMultibodyLink::ePrismatic)
+					{
+						btVector3 vec = quatRotate(tr.getRotation(), bod->getLink(m).m_axes[0].m_bottomVec) * 0.1;
+
+						btVector4 color(0, 0, 0, 1);  //1,1,1);
+						btVector3 from = vec + tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						btVector3 to = tr.getOrigin() - quatRotate(tr.getRotation(), bod->getLink(m).m_dVector);
+						getDebugDrawer()->drawLine(from, to, color);
 					}
-					
 				}
 			}
 		}
 	}
-
-	btDiscreteDynamicsWorld::debugDrawWorld();
 }
 
-
-
 void btMultiBodyDynamicsWorld::applyGravity()
 {
-        btDiscreteDynamicsWorld::applyGravity();
+	btDiscreteDynamicsWorld::applyGravity();
 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-        BT_PROFILE("btMultiBody addGravity");
-        for (int i=0;i<this->m_multiBodies.size();i++)
-        {
-                btMultiBody* bod = m_multiBodies[i];
+	BT_PROFILE("btMultiBody addGravity");
+	for (int i = 0; i < this->m_multiBodies.size(); i++)
+	{
+		btMultiBody* bod = m_multiBodies[i];
 
-                bool isSleeping = false;
+		bool isSleeping = false;
 
-                if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
-                {
-                        isSleeping = true;
-                }
-                for (int b=0;b<bod->getNumLinks();b++)
-                {
-                        if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
-                                isSleeping = true;
-                }
+		if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+		{
+			isSleeping = true;
+		}
+		for (int b = 0; b < bod->getNumLinks(); b++)
+		{
+			if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+				isSleeping = true;
+		}
 
-                if (!isSleeping)
-                {
-                        bod->addBaseForce(m_gravity * bod->getBaseMass());
+		if (!isSleeping)
+		{
+			bod->addBaseForce(m_gravity * bod->getBaseMass());
 
-                        for (int j = 0; j < bod->getNumLinks(); ++j)
-                        {
-                                bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
-                        }
-                }//if (!isSleeping)
-        }
-#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+			for (int j = 0; j < bod->getNumLinks(); ++j)
+			{
+				bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
+			}
+		}  //if (!isSleeping)
+	}
+#endif  //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 }
 
 void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces()
-{ 
-  for (int i=0;i<this->m_multiBodies.size();i++)
-                {       
-                        btMultiBody* bod = m_multiBodies[i];
-			bod->clearConstraintForces();
-                  } 
+{
+	for (int i = 0; i < this->m_multiBodies.size(); i++)
+	{
+		btMultiBody* bod = m_multiBodies[i];
+		bod->clearConstraintForces();
+	}
 }
 void btMultiBodyDynamicsWorld::clearMultiBodyForces()
 {
-              {
-                BT_PROFILE("clearMultiBodyForces");
-                for (int i=0;i<this->m_multiBodies.size();i++)
-                {
-                        btMultiBody* bod = m_multiBodies[i];
+	{
+		// BT_PROFILE("clearMultiBodyForces");
+		for (int i = 0; i < this->m_multiBodies.size(); i++)
+		{
+			btMultiBody* bod = m_multiBodies[i];
 
-                        bool isSleeping = false;
+			bool isSleeping = false;
 
-                        if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
-                        {       
-                                isSleeping = true;
-                        }
-                        for (int b=0;b<bod->getNumLinks();b++)
-                        {       
-                                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)     
-                                        isSleeping = true;
-                        }
+			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+			{
+				isSleeping = true;
+			}
+			for (int b = 0; b < bod->getNumLinks(); b++)
+			{
+				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+					isSleeping = true;
+			}
 
-                        if (!isSleeping)
-                        {
-                        btMultiBody* bod = m_multiBodies[i];
-                        bod->clearForcesAndTorques();
-                	}
+			if (!isSleeping)
+			{
+				btMultiBody* bod = m_multiBodies[i];
+				bod->clearForcesAndTorques();
+			}
 		}
 	}
-
 }
 void btMultiBodyDynamicsWorld::clearForces()
 {
-        btDiscreteDynamicsWorld::clearForces();
+	btDiscreteDynamicsWorld::clearForces();
 
 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 	clearMultiBodyForces();
 #endif
 }
 
-
-
-
-void	btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
+void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
 {
-
 	serializer->startSerialization();
 
-	serializeDynamicsWorldInfo( serializer);
+	serializeDynamicsWorldInfo(serializer);
 
 	serializeMultiBodies(serializer);
 
@@ -977,22 +836,41 @@ void	btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
 
 	serializeCollisionObjects(serializer);
 
+	serializeContactManifolds(serializer);
+
 	serializer->finishSerialization();
 }
 
-void	btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
+void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
 {
 	int i;
 	//serialize all collision objects
-	for (i=0;i<m_multiBodies.size();i++)
+	for (i = 0; i < m_multiBodies.size(); i++)
 	{
 		btMultiBody* mb = m_multiBodies[i];
 		{
 			int len = mb->calculateSerializeBufferSize();
-			btChunk* chunk = serializer->allocate(len,1);
+			btChunk* chunk = serializer->allocate(len, 1);
 			const char* structType = mb->serialize(chunk->m_oldPtr, serializer);
-			serializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
+			serializer->finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb);
 		}
 	}
 
-}
\ No newline at end of file
+	//serialize all multibody links (collision objects)
+	for (i = 0; i < m_collisionObjects.size(); i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		if (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+		{
+			int len = colObj->calculateSerializeBufferSize();
+			btChunk* chunk = serializer->allocate(len, 1);
+			const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk, structType, BT_MB_LINKCOLLIDER_CODE, colObj);
+		}
+	}
+}
+//
+//void btMultiBodyDynamicsWorld::setSplitIslands(bool split)
+//{
+//    m_islandManager->setSplitIslands(split);
+//}
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
index 03ef3335c22..9ac46f4b64f 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
@@ -17,6 +17,7 @@ subject to the following restrictions:
 #define BT_MULTIBODY_DYNAMICS_WORLD_H
 
 #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h"
 
 #define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
 
@@ -33,67 +34,91 @@ protected:
 	btAlignedObjectArray<btMultiBody*> m_multiBodies;
 	btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
 	btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints;
-	btMultiBodyConstraintSolver*	m_multiBodyConstraintSolver;
-	MultiBodyInplaceSolverIslandCallback*	m_solverMultiBodyIslandCallback;
-
-	virtual void	calculateSimulationIslands();
-	virtual void	updateActivationState(btScalar timeStep);
-	virtual void	solveConstraints(btContactSolverInfo& solverInfo);
+	btMultiBodyConstraintSolver* m_multiBodyConstraintSolver;
+	MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback;
+
+	//cached data to avoid memory allocations
+	btAlignedObjectArray<btQuaternion> m_scratch_world_to_local;
+	btAlignedObjectArray<btVector3> m_scratch_local_origin;
+	btAlignedObjectArray<btQuaternion> m_scratch_world_to_local1;
+	btAlignedObjectArray<btVector3> m_scratch_local_origin1;
+	btAlignedObjectArray<btScalar> m_scratch_r;
+	btAlignedObjectArray<btVector3> m_scratch_v;
+	btAlignedObjectArray<btMatrix3x3> m_scratch_m;
+
+	virtual void calculateSimulationIslands();
+	virtual void updateActivationState(btScalar timeStep);
 	
-	virtual void	serializeMultiBodies(btSerializer* serializer);
-
-public:
 
-	btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+	virtual void serializeMultiBodies(btSerializer* serializer);
 
-	virtual ~btMultiBodyDynamicsWorld ();
+public:
+	btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration);
 
-	virtual void	addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter);
+	virtual ~btMultiBodyDynamicsWorld();
+    
+    virtual void solveConstraints(btContactSolverInfo& solverInfo);
+    
+	virtual void addMultiBody(btMultiBody* body, int group = btBroadphaseProxy::DefaultFilter, int mask = btBroadphaseProxy::AllFilter);
 
-	virtual void	removeMultiBody(btMultiBody* body);
+	virtual void removeMultiBody(btMultiBody* body);
 
-	virtual int		getNumMultibodies() const
+	virtual int getNumMultibodies() const
 	{
 		return m_multiBodies.size();
 	}
 
-	btMultiBody*	getMultiBody(int mbIndex)
+	btMultiBody* getMultiBody(int mbIndex)
+	{
+		return m_multiBodies[mbIndex];
+	}
+
+	const btMultiBody* getMultiBody(int mbIndex) const
 	{
 		return m_multiBodies[mbIndex];
 	}
 
-	virtual void	addMultiBodyConstraint( btMultiBodyConstraint* constraint);
+	virtual void addMultiBodyConstraint(btMultiBodyConstraint* constraint);
 
-	virtual int     getNumMultiBodyConstraints() const
+	virtual int getNumMultiBodyConstraints() const
 	{
-        return m_multiBodyConstraints.size();
+		return m_multiBodyConstraints.size();
 	}
 
-	virtual btMultiBodyConstraint*	getMultiBodyConstraint( int constraintIndex)
+	virtual btMultiBodyConstraint* getMultiBodyConstraint(int constraintIndex)
 	{
-        return m_multiBodyConstraints[constraintIndex];
+		return m_multiBodyConstraints[constraintIndex];
 	}
 
-	virtual const btMultiBodyConstraint*	getMultiBodyConstraint( int constraintIndex) const
+	virtual const btMultiBodyConstraint* getMultiBodyConstraint(int constraintIndex) const
 	{
-        return m_multiBodyConstraints[constraintIndex];
+		return m_multiBodyConstraints[constraintIndex];
 	}
 
-	virtual void	removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
+	virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint);
 
-	virtual void	integrateTransforms(btScalar timeStep);
+	virtual void integrateTransforms(btScalar timeStep);
+    void integrateMultiBodyTransforms(btScalar timeStep);
+    void predictMultiBodyTransforms(btScalar timeStep);
+    
+    virtual void predictUnconstraintMotion(btScalar timeStep);
+	virtual void debugDrawWorld();
 
-	virtual void	debugDrawWorld();
+	virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
 
-	virtual void	debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
-	
-	void	forwardKinematics();
+	void forwardKinematics();
 	virtual void clearForces();
 	virtual void clearMultiBodyConstraintForces();
 	virtual void clearMultiBodyForces();
 	virtual void applyGravity();
-	
-	virtual	void	serialize(btSerializer* serializer);
 
+	virtual void serialize(btSerializer* serializer);
+	virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver);
+	virtual void setConstraintSolver(btConstraintSolver* solver);
+	virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const;
+    
+    virtual void solveExternalForces(btContactSolverInfo& solverInfo);
+    virtual void solveInternalConstraints(btContactSolverInfo& solverInfo);
+    void buildIslands();
 };
-#endif //BT_MULTIBODY_DYNAMICS_WORLD_H
+#endif  //BT_MULTIBODY_DYNAMICS_WORLD_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
new file mode 100644
index 00000000000..df2abbe97a5
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
@@ -0,0 +1,215 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodyFixedConstraint.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#define BTMBFIXEDCONSTRAINT_DIM 6
+
+btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
+	: btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(bodyB),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB)
+{
+	m_data.resize(BTMBFIXEDCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(0),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB)
+{
+	m_data.resize(BTMBFIXEDCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+void btMultiBodyFixedConstraint::finalizeMultiDof()
+{
+	//not implemented yet
+	btAssert(0);
+}
+
+btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint()
+{
+}
+
+int btMultiBodyFixedConstraint::getIslandIdA() const
+{
+	if (m_rigidBodyA)
+		return m_rigidBodyA->getIslandTag();
+
+	if (m_bodyA)
+	{
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodyFixedConstraint::getIslandIdB() const
+{
+	if (m_rigidBodyB)
+		return m_rigidBodyB->getIslandTag();
+	if (m_bodyB)
+	{
+		if (m_linkB < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
+{
+	int numDim = BTMBFIXEDCONSTRAINT_DIM;
+	for (int i = 0; i < numDim; i++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = i;
+		constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal1.setValue(0, 0, 0);
+		constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal2.setValue(0, 0, 0);
+		constraintRow.m_angularComponentA.setValue(0, 0, 0);
+		constraintRow.m_angularComponentB.setValue(0, 0, 0);
+
+		constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
+		constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
+
+		// Convert local points back to world
+		btVector3 pivotAworld = m_pivotInA;
+		btMatrix3x3 frameAworld = m_frameInA;
+		if (m_rigidBodyA)
+		{
+			constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
+			pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+			frameAworld = frameAworld.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
+		}
+		else
+		{
+			if (m_bodyA)
+			{
+				pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+				frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
+			}
+		}
+		btVector3 pivotBworld = m_pivotInB;
+		btMatrix3x3 frameBworld = m_frameInB;
+		if (m_rigidBodyB)
+		{
+			constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
+			pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+			frameBworld = frameBworld.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
+		}
+		else
+		{
+			if (m_bodyB)
+			{
+				pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+				frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld);
+			}
+		}
+
+		btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
+		btVector3 angleDiff;
+		btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
+
+		btVector3 constraintNormalLin(0, 0, 0);
+		btVector3 constraintNormalAng(0, 0, 0);
+		btScalar posError = 0.0;
+		if (i < 3)
+		{
+			constraintNormalLin[i] = 1;
+			posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
+			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+									constraintNormalLin, pivotAworld, pivotBworld,
+									posError,
+									infoGlobal,
+									-m_maxAppliedImpulse, m_maxAppliedImpulse);
+		}
+		else
+		{  //i>=3
+			constraintNormalAng = frameAworld.getColumn(i % 3);
+			posError = angleDiff[i % 3];
+			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+									constraintNormalLin, pivotAworld, pivotBworld,
+									posError,
+									infoGlobal,
+									-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
+		}
+	}
+}
+
+void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer)
+{
+	btTransform tr;
+	tr.setIdentity();
+
+	if (m_rigidBodyA)
+	{
+		btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyA)
+	{
+		btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+		tr.setOrigin(pivotAworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_rigidBodyB)
+	{
+		// that ideally should draw the same frame
+		btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyB)
+	{
+		btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+		tr.setOrigin(pivotBworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+}
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h
new file mode 100644
index 00000000000..adb1cb47dad
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h
@@ -0,0 +1,91 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_FIXED_CONSTRAINT_H
+#define BT_MULTIBODY_FIXED_CONSTRAINT_H
+
+#include "btMultiBodyConstraint.h"
+
+class btMultiBodyFixedConstraint : public btMultiBodyConstraint
+{
+protected:
+	btRigidBody* m_rigidBodyA;
+	btRigidBody* m_rigidBodyB;
+	btVector3 m_pivotInA;
+	btVector3 m_pivotInB;
+	btMatrix3x3 m_frameInA;
+	btMatrix3x3 m_frameInB;
+
+public:
+	btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+	btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+
+	virtual ~btMultiBodyFixedConstraint();
+
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	const btVector3& getPivotInA() const
+	{
+		return m_pivotInA;
+	}
+
+	void setPivotInA(const btVector3& pivotInA)
+	{
+		m_pivotInA = pivotInA;
+	}
+
+	const btVector3& getPivotInB() const
+	{
+		return m_pivotInB;
+	}
+
+	virtual void setPivotInB(const btVector3& pivotInB)
+	{
+		m_pivotInB = pivotInB;
+	}
+
+	const btMatrix3x3& getFrameInA() const
+	{
+		return m_frameInA;
+	}
+
+	void setFrameInA(const btMatrix3x3& frameInA)
+	{
+		m_frameInA = frameInA;
+	}
+
+	const btMatrix3x3& getFrameInB() const
+	{
+		return m_frameInB;
+	}
+
+	virtual void setFrameInB(const btMatrix3x3& frameInB)
+	{
+		m_frameInB = frameInB;
+	}
+
+	virtual void debugDraw(class btIDebugDraw* drawer);
+};
+
+#endif  //BT_MULTIBODY_FIXED_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
new file mode 100644
index 00000000000..ee02cf9b07a
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
@@ -0,0 +1,181 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodyGearConstraint.h"
+#include "btMultiBody.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+
+btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false, MULTIBODY_CONSTRAINT_GEAR),
+	  m_gearRatio(1),
+	  m_gearAuxLink(-1),
+	  m_erp(0),
+	  m_relativePositionTarget(0)
+{
+}
+
+void btMultiBodyGearConstraint::finalizeMultiDof()
+{
+	allocateJacobiansMultiDof();
+
+	m_numDofsFinalized = m_jacSizeBoth;
+}
+
+btMultiBodyGearConstraint::~btMultiBodyGearConstraint()
+{
+}
+
+int btMultiBodyGearConstraint::getIslandIdA() const
+{
+	if (m_bodyA)
+	{
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodyGearConstraint::getIslandIdB() const
+{
+	if (m_bodyB)
+	{
+		if (m_linkB < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+													 btMultiBodyJacobianData& data,
+													 const btContactSolverInfo& infoGlobal)
+{
+	// only positions need to be updated -- data.m_jacobians and force
+	// directions were set in the ctor and never change.
+
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+		finalizeMultiDof();
+	}
+
+	//don't crash
+	if (m_numDofsFinalized != m_jacSizeBoth)
+		return;
+
+	if (m_maxAppliedImpulse == 0.f)
+		return;
+
+	// note: we rely on the fact that data.m_jacobians are
+	// always initialized to zero by the Constraint ctor
+	int linkDoF = 0;
+	unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
+	unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF);
+
+	// row 0: the lower bound
+	jacobianA(0)[offsetA] = 1;
+	jacobianB(0)[offsetB] = m_gearRatio;
+
+	btScalar posError = 0;
+	const btVector3 dummy(0, 0, 0);
+
+	btScalar kp = 1;
+	btScalar kd = 1;
+	int numRows = getNumRows();
+
+	for (int row = 0; row < numRows; row++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+
+		int dof = 0;
+		btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+		btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+		btScalar auxVel = 0;
+
+		if (m_gearAuxLink >= 0)
+		{
+			auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof];
+		}
+		currentVelocity += auxVel;
+		if (m_erp != 0)
+		{
+			btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+			if (m_gearAuxLink >= 0)
+			{
+				currentPositionA -= m_bodyA->getJointPosMultiDof(m_gearAuxLink)[dof];
+			}
+			btScalar currentPositionB = m_gearRatio * m_bodyA->getJointPosMultiDof(m_linkB)[dof];
+			btScalar diff = currentPositionB + currentPositionA;
+			btScalar desiredPositionDiff = this->m_relativePositionTarget;
+			posError = -m_erp * (desiredPositionDiff - diff);
+		}
+
+		btScalar desiredRelativeVelocity = auxVel;
+
+		fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, desiredRelativeVelocity);
+
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = row;
+		{
+			//expect either prismatic or revolute joint type for now
+			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
+			switch (m_bodyA->getLink(m_linkA).m_jointType)
+			{
+				case btMultibodyLink::eRevolute:
+				{
+					constraintRow.m_contactNormal1.setZero();
+					constraintRow.m_contactNormal2.setZero();
+					btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
+					constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
+					constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
+
+					break;
+				}
+				case btMultibodyLink::ePrismatic:
+				{
+					btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
+					constraintRow.m_contactNormal1 = prismaticAxisInWorld;
+					constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
+					constraintRow.m_relpos1CrossNormal.setZero();
+					constraintRow.m_relpos2CrossNormal.setZero();
+					break;
+				}
+				default:
+				{
+					btAssert(0);
+				}
+			};
+		}
+	}
+}
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h
new file mode 100644
index 00000000000..31888fbc682
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h
@@ -0,0 +1,115 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_GEAR_CONSTRAINT_H
+#define BT_MULTIBODY_GEAR_CONSTRAINT_H
+
+#include "btMultiBodyConstraint.h"
+
+class btMultiBodyGearConstraint : public btMultiBodyConstraint
+{
+protected:
+	btRigidBody* m_rigidBodyA;
+	btRigidBody* m_rigidBodyB;
+	btVector3 m_pivotInA;
+	btVector3 m_pivotInB;
+	btMatrix3x3 m_frameInA;
+	btMatrix3x3 m_frameInB;
+	btScalar m_gearRatio;
+	int m_gearAuxLink;
+	btScalar m_erp;
+	btScalar m_relativePositionTarget;
+
+public:
+	//btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+	btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+
+	virtual ~btMultiBodyGearConstraint();
+
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	const btVector3& getPivotInA() const
+	{
+		return m_pivotInA;
+	}
+
+	void setPivotInA(const btVector3& pivotInA)
+	{
+		m_pivotInA = pivotInA;
+	}
+
+	const btVector3& getPivotInB() const
+	{
+		return m_pivotInB;
+	}
+
+	virtual void setPivotInB(const btVector3& pivotInB)
+	{
+		m_pivotInB = pivotInB;
+	}
+
+	const btMatrix3x3& getFrameInA() const
+	{
+		return m_frameInA;
+	}
+
+	void setFrameInA(const btMatrix3x3& frameInA)
+	{
+		m_frameInA = frameInA;
+	}
+
+	const btMatrix3x3& getFrameInB() const
+	{
+		return m_frameInB;
+	}
+
+	virtual void setFrameInB(const btMatrix3x3& frameInB)
+	{
+		m_frameInB = frameInB;
+	}
+
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
+
+	virtual void setGearRatio(btScalar gearRatio)
+	{
+		m_gearRatio = gearRatio;
+	}
+	virtual void setGearAuxLink(int gearAuxLink)
+	{
+		m_gearAuxLink = gearAuxLink;
+	}
+	virtual void setRelativePositionTarget(btScalar relPosTarget)
+	{
+		m_relativePositionTarget = relPosTarget;
+	}
+	virtual void setErp(btScalar erp)
+	{
+		m_erp = erp;
+	}
+};
+
+#endif  //BT_MULTIBODY_GEAR_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h
new file mode 100644
index 00000000000..3169b86e616
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h
@@ -0,0 +1,247 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H
+#define BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H
+
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "btMultiBodyConstraintSolver.h"
+
+SIMD_FORCE_INLINE int btGetConstraintIslandId2(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;
+}
+class btSortConstraintOnIslandPredicate2
+{
+public:
+    bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const
+    {
+        int rIslandId0, lIslandId0;
+        rIslandId0 = btGetConstraintIslandId2(rhs);
+        lIslandId0 = btGetConstraintIslandId2(lhs);
+        return lIslandId0 < rIslandId0;
+    }
+};
+
+SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs)
+{
+    int islandId;
+    
+    int islandTagA = lhs->getIslandIdA();
+    int islandTagB = lhs->getIslandIdB();
+    islandId = islandTagA >= 0 ? islandTagA : islandTagB;
+    return islandId;
+}
+
+class btSortMultiBodyConstraintOnIslandPredicate
+{
+public:
+    bool operator()(const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs) const
+    {
+        int rIslandId0, lIslandId0;
+        rIslandId0 = btGetMultiBodyConstraintIslandId(rhs);
+        lIslandId0 = btGetMultiBodyConstraintIslandId(lhs);
+        return lIslandId0 < rIslandId0;
+    }
+};
+
+struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
+{
+
+    btContactSolverInfo* m_solverInfo;
+    btMultiBodyConstraintSolver* m_solver;
+    btMultiBodyConstraint** m_multiBodySortedConstraints;
+    int m_numMultiBodyConstraints;
+    
+    btTypedConstraint** m_sortedConstraints;
+    int m_numConstraints;
+    btIDebugDraw* m_debugDrawer;
+    btDispatcher* m_dispatcher;
+    
+    btAlignedObjectArray<btCollisionObject*> m_bodies;
+	btAlignedObjectArray<btCollisionObject*> m_softBodies;
+    btAlignedObjectArray<btPersistentManifold*> m_manifolds;
+    btAlignedObjectArray<btTypedConstraint*> m_constraints;
+    btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
+    
+    btAlignedObjectArray<btSolverAnalyticsData> m_islandAnalyticsData;
+    
+    MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver,
+                                         btDispatcher* dispatcher)
+    : m_solverInfo(NULL),
+    m_solver(solver),
+    m_multiBodySortedConstraints(NULL),
+    m_numConstraints(0),
+    m_debugDrawer(NULL),
+    m_dispatcher(dispatcher)
+    {
+    }
+    
+    MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other)
+    {
+        btAssert(0);
+        (void)other;
+        return *this;
+    }
+    
+    SIMD_FORCE_INLINE virtual void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
+    {
+        m_islandAnalyticsData.clear();
+        btAssert(solverInfo);
+        m_solverInfo = solverInfo;
+        
+        m_multiBodySortedConstraints = sortedMultiBodyConstraints;
+        m_numMultiBodyConstraints = numMultiBodyConstraints;
+        m_sortedConstraints = sortedConstraints;
+        m_numConstraints = numConstraints;
+        
+        m_debugDrawer = debugDrawer;
+        m_bodies.resize(0);
+        m_manifolds.resize(0);
+        m_constraints.resize(0);
+        m_multiBodyConstraints.resize(0);
+    }
+    
+    void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
+    {
+        m_solver = solver;
+    }
+    
+    virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId)
+    {
+        if (islandId < 0)
+        {
+            ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
+            m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
+            if (m_solverInfo->m_reportSolverAnalytics&1)
+            {
+                m_solver->m_analyticsData.m_islandId = islandId;
+                m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
+            }
+        }
+        else
+        {
+            //also add all non-contact constraints/joints for this island
+            btTypedConstraint** startConstraint = 0;
+            btMultiBodyConstraint** startMultiBodyConstraint = 0;
+            
+            int numCurConstraints = 0;
+            int numCurMultiBodyConstraints = 0;
+            
+            int i;
+            
+            //find the first constraint for this island
+            
+            for (i = 0; i < m_numConstraints; i++)
+            {
+                if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
+                {
+                    startConstraint = &m_sortedConstraints[i];
+                    break;
+                }
+            }
+            //count the number of constraints in this island
+            for (; i < m_numConstraints; i++)
+            {
+                if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
+                {
+                    numCurConstraints++;
+                }
+            }
+            
+            for (i = 0; i < m_numMultiBodyConstraints; i++)
+            {
+                if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
+                {
+                    startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
+                    break;
+                }
+            }
+            //count the number of multi body constraints in this island
+            for (; i < m_numMultiBodyConstraints; i++)
+            {
+                if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
+                {
+                    numCurMultiBodyConstraints++;
+                }
+            }
+            
+            //if (m_solverInfo->m_minimumSolverBatchSize<=1)
+            //{
+            //    m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
+            //} else
+            {
+                for (i = 0; i < numBodies; i++)
+				{
+					bool isSoftBodyType = (bodies[i]->getInternalType() & btCollisionObject::CO_SOFT_BODY);
+					if (!isSoftBodyType)
+					{
+						m_bodies.push_back(bodies[i]);
+					}
+					else
+					{
+						m_softBodies.push_back(bodies[i]);
+					}
+				}
+                for (i = 0; i < numManifolds; i++)
+                    m_manifolds.push_back(manifolds[i]);
+                for (i = 0; i < numCurConstraints; i++)
+                    m_constraints.push_back(startConstraint[i]);
+                
+                for (i = 0; i < numCurMultiBodyConstraints; i++)
+                    m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
+                
+                if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize)
+                {
+                    processConstraints(islandId);
+                }
+                else
+                {
+                    //printf("deferred\n");
+                }
+            }
+        }
+    }
+    
+    virtual void processConstraints(int islandId=-1)
+    {
+        btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
+        btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
+        btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
+        btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
+        
+        //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
+        
+        m_solver->solveMultiBodyGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
+        if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1))
+        {
+            m_solver->m_analyticsData.m_islandId = islandId;
+            m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
+        }
+        m_bodies.resize(0);
+		m_softBodies.resize(0);
+        m_manifolds.resize(0);
+        m_constraints.resize(0);
+        m_multiBodyConstraints.resize(0);
+    }
+};
+
+
+#endif /*BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H */
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
index 5c2fa8ed5b9..d943019e711 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
@@ -12,8 +12,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
-
 #ifndef BT_MULTIBODY_JOINT_FEEDBACK_H
 #define BT_MULTIBODY_JOINT_FEEDBACK_H
 
@@ -21,7 +19,7 @@ subject to the following restrictions:
 
 struct btMultiBodyJointFeedback
 {
-	btSpatialForceVector	m_reactionForces;
+	btSpatialForceVector m_reactionForces;
 };
 
-#endif //BT_MULTIBODY_JOINT_FEEDBACK_H
+#endif  //BT_MULTIBODY_JOINT_FEEDBACK_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
index 3f05aa4d5fa..94b36ac108c 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
@@ -20,21 +20,18 @@ subject to the following restrictions:
 #include "btMultiBodyLinkCollider.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
-
-
 btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
 	//:btMultiBodyConstraint(body,0,link,-1,2,true),
-	:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,2,true),
-	m_lowerBound(lower),
-	m_upperBound(upper)
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true, MULTIBODY_CONSTRAINT_LIMIT),
+	  m_lowerBound(lower),
+	  m_upperBound(upper)
 {
-
 }
 
 void btMultiBodyJointLimitConstraint::finalizeMultiDof()
 {
 	// the data.m_jacobians never change, so may as well
-    // initialize them here
+	// initialize them here
 
 	allocateJacobiansMultiDof();
 
@@ -55,15 +52,18 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
 
 int btMultiBodyJointLimitConstraint::getIslandIdA() const
 {
-	if(m_bodyA)
+	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
 		}
 	}
 	return -1;
@@ -71,82 +71,86 @@ int btMultiBodyJointLimitConstraint::getIslandIdA() const
 
 int btMultiBodyJointLimitConstraint::getIslandIdB() const
 {
-	if(m_bodyB)
+	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }
 
-
 void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal)
+														   btMultiBodyJacobianData& data,
+														   const btContactSolverInfo& infoGlobal)
 {
-	
-    // only positions need to be updated -- data.m_jacobians and force
-    // directions were set in the ctor and never change.
+	// only positions need to be updated -- data.m_jacobians and force
+	// directions were set in the ctor and never change.
 
 	if (m_numDofsFinalized != m_jacSizeBoth)
 	{
-        finalizeMultiDof();
+		finalizeMultiDof();
 	}
 
+	// row 0: the lower bound
+	setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);  //multidof: this is joint-type dependent
 
-    // row 0: the lower bound
-    setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);			//multidof: this is joint-type dependent
+	// row 1: the upper bound
+	setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
 
-    // row 1: the upper bound
-    setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
-	
-	for (int row=0;row<getNumRows();row++)
+	for (int row = 0; row < getNumRows(); row++)
 	{
-		
-		btScalar direction = row? -1 : 1;
+		btScalar penetration = getPosition(row);
+
+		//todo: consider adding some safety threshold here
+		if (penetration > 0)
+		{
+			continue;
+		}
+		btScalar direction = row ? -1 : 1;
 
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
-        constraintRow.m_orgConstraint = this;
-        constraintRow.m_orgDofIndex = row;
-        
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = row;
+
 		constraintRow.m_multiBodyA = m_bodyA;
 		constraintRow.m_multiBodyB = m_bodyB;
-		const btScalar posError = 0;						//why assume it's zero?
+		const btScalar posError = 0;  //why assume it's zero?
 		const btVector3 dummy(0, 0, 0);
 
-		btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
+		btScalar rel_vel = fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, 0, m_maxAppliedImpulse);
 
 		{
 			//expect either prismatic or revolute joint type for now
-			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
+			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
 			switch (m_bodyA->getLink(m_linkA).m_jointType)
 			{
 				case btMultibodyLink::eRevolute:
 				{
 					constraintRow.m_contactNormal1.setZero();
 					constraintRow.m_contactNormal2.setZero();
-					btVector3 revoluteAxisInWorld = direction*quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
-					constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
-					constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
-					
+					btVector3 revoluteAxisInWorld = direction * quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
+					constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
+					constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
+
 					break;
 				}
 				case btMultibodyLink::ePrismatic:
 				{
-					btVector3 prismaticAxisInWorld = direction* quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
-					constraintRow.m_contactNormal1=prismaticAxisInWorld;
-					constraintRow.m_contactNormal2=-prismaticAxisInWorld;
+					btVector3 prismaticAxisInWorld = direction * quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
+					constraintRow.m_contactNormal1 = prismaticAxisInWorld;
+					constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
 					constraintRow.m_relpos1CrossNormal.setZero();
 					constraintRow.m_relpos2CrossNormal.setZero();
-					
+
 					break;
 				}
 				default:
@@ -154,36 +158,35 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 					btAssert(0);
 				}
 			};
-			
 		}
 
 		{
-			btScalar penetration = getPosition(row);
 			btScalar positionalError = 0.f;
-			btScalar	velocityError =  - rel_vel;// * damping;
+			btScalar velocityError = -rel_vel;  // * damping;
 			btScalar erp = infoGlobal.m_erp2;
 			if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 			{
 				erp = infoGlobal.m_erp;
 			}
-			if (penetration>0)
+			if (penetration > 0)
 			{
 				positionalError = 0;
 				velocityError = -penetration / infoGlobal.m_timeStep;
-			} else
+			}
+			else
 			{
-				positionalError = -penetration * erp/infoGlobal.m_timeStep;
+				positionalError = -penetration * erp / infoGlobal.m_timeStep;
 			}
 
-			btScalar  penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
-			btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
+			btScalar penetrationImpulse = positionalError * constraintRow.m_jacDiagABInv;
+			btScalar velocityImpulse = velocityError * constraintRow.m_jacDiagABInv;
 			if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 			{
 				//combine position and velocity into rhs
-				constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
+				constraintRow.m_rhs = penetrationImpulse + velocityImpulse;
 				constraintRow.m_rhsPenetration = 0.f;
-
-			} else
+			}
+			else
 			{
 				//split position and velocity into rhs and m_rhsPenetration
 				constraintRow.m_rhs = velocityImpulse;
@@ -191,9 +194,4 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 			}
 		}
 	}
-
 }
-
-
-
-
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h
index 55b8d122b97..b810692b4ce 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h
@@ -22,11 +22,10 @@ struct btSolverInfo;
 class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint
 {
 protected:
+	btScalar m_lowerBound;
+	btScalar m_upperBound;
 
-	btScalar	m_lowerBound;
-	btScalar	m_upperBound;
 public:
-
 	btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
 	virtual ~btMultiBodyJointLimitConstraint();
 
@@ -36,15 +35,29 @@ public:
 	virtual int getIslandIdB() const;
 
 	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal);
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
 
 	virtual void debugDraw(class btIDebugDraw* drawer)
 	{
 		//todo(erwincoumans)
 	}
-
+	btScalar getLowerBound() const
+	{
+		return m_lowerBound;
+	}
+	btScalar getUpperBound() const
+	{
+		return m_upperBound;
+	}
+	void setLowerBound(btScalar lower)
+	{
+		m_lowerBound = lower;
+	}
+	void setUpperBound(btScalar upper)
+	{
+		m_upperBound = upper;
+	}
 };
 
-#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
-
+#endif  //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
index 062d19accaa..fec9b03213a 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
@@ -20,17 +20,18 @@ subject to the following restrictions:
 #include "btMultiBodyLinkCollider.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
-
 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
-	:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
-	m_desiredVelocity(desiredVelocity)
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR),
+	  m_desiredVelocity(desiredVelocity),
+	  m_desiredPosition(0),
+	  m_kd(1.),
+	  m_kp(0),
+	  m_erp(1),
+	  m_rhsClamp(SIMD_INFINITY)
 {
-
 	m_maxAppliedImpulse = maxMotorImpulse;
 	// the data.m_jacobians never change, so may as well
-    // initialize them here
-
-
+	// initialize them here
 }
 
 void btMultiBodyJointMotor::finalizeMultiDof()
@@ -50,13 +51,17 @@ void btMultiBodyJointMotor::finalizeMultiDof()
 
 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
 	//:btMultiBodyConstraint(body,0,link,-1,1,true),
-	:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
-	m_desiredVelocity(desiredVelocity)
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR),
+	  m_desiredVelocity(desiredVelocity),
+	  m_desiredPosition(0),
+	  m_kd(1.),
+	  m_kp(0),
+	  m_erp(1),
+	  m_rhsClamp(SIMD_INFINITY)
 {
 	btAssert(linkDoF < body->getLink(link).m_dofCount);
 
 	m_maxAppliedImpulse = maxMotorImpulse;
-
 }
 btMultiBodyJointMotor::~btMultiBodyJointMotor()
 {
@@ -64,83 +69,108 @@ btMultiBodyJointMotor::~btMultiBodyJointMotor()
 
 int btMultiBodyJointMotor::getIslandIdA() const
 {
-	btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-	if (col)
-		return col->getIslandTag();
-	for (int i=0;i<m_bodyA->getNumLinks();i++)
+	if (this->m_linkA < 0)
 	{
-		if (m_bodyA->getLink(i).m_collider)
-			return m_bodyA->getLink(i).m_collider->getIslandTag();
+		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyA->getLink(m_linkA).m_collider)
+		{
+			return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }
 
 int btMultiBodyJointMotor::getIslandIdB() const
 {
-	btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-	if (col)
-		return col->getIslandTag();
-
-	for (int i=0;i<m_bodyB->getNumLinks();i++)
+	if (m_linkB < 0)
 	{
-		col = m_bodyB->getLink(i).m_collider;
+		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 		if (col)
 			return col->getIslandTag();
 	}
+	else
+	{
+		if (m_bodyB->getLink(m_linkB).m_collider)
+		{
+			return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
 	return -1;
 }
 
-
 void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal)
+												 btMultiBodyJacobianData& data,
+												 const btContactSolverInfo& infoGlobal)
 {
-    // only positions need to be updated -- data.m_jacobians and force
-    // directions were set in the ctor and never change.
-	
+	// only positions need to be updated -- data.m_jacobians and force
+	// directions were set in the ctor and never change.
+
 	if (m_numDofsFinalized != m_jacSizeBoth)
 	{
-        finalizeMultiDof();
+		finalizeMultiDof();
 	}
 
 	//don't crash
 	if (m_numDofsFinalized != m_jacSizeBoth)
 		return;
 
+	if (m_maxAppliedImpulse == 0.f)
+		return;
+
 	const btScalar posError = 0;
 	const btVector3 dummy(0, 0, 0);
 
-	for (int row=0;row<getNumRows();row++)
+	for (int row = 0; row < getNumRows(); row++)
 	{
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
 
+		int dof = 0;
+		btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+		btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+		btScalar positionStabiliationTerm = m_erp * (m_desiredPosition - currentPosition) / infoGlobal.m_timeStep;
 
-		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
+		btScalar velocityError = (m_desiredVelocity - currentVelocity);
+		btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity + m_kd * velocityError;
+		if (rhs > m_rhsClamp)
+		{
+			rhs = m_rhsClamp;
+		}
+		if (rhs < -m_rhsClamp)
+		{
+			rhs = -m_rhsClamp;
+		}
+
+		fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, rhs);
 		constraintRow.m_orgConstraint = this;
 		constraintRow.m_orgDofIndex = row;
 		{
 			//expect either prismatic or revolute joint type for now
-			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
+			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
 			switch (m_bodyA->getLink(m_linkA).m_jointType)
 			{
 				case btMultibodyLink::eRevolute:
 				{
 					constraintRow.m_contactNormal1.setZero();
 					constraintRow.m_contactNormal2.setZero();
-					btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
-					constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
-					constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
-					
+					btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
+					constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
+					constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
+
 					break;
 				}
 				case btMultibodyLink::ePrismatic:
 				{
-					btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
-					constraintRow.m_contactNormal1=prismaticAxisInWorld;
-					constraintRow.m_contactNormal2=-prismaticAxisInWorld;
+					btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
+					constraintRow.m_contactNormal1 = prismaticAxisInWorld;
+					constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
 					constraintRow.m_relpos1CrossNormal.setZero();
 					constraintRow.m_relpos2CrossNormal.setZero();
-					
+
 					break;
 				}
 				default:
@@ -148,10 +178,6 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
 					btAssert(0);
 				}
 			};
-			
 		}
-
 	}
-
 }
-
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
index 011aadcfa4b..1aca36352e7 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
@@ -24,34 +24,54 @@ struct btSolverInfo;
 class btMultiBodyJointMotor : public btMultiBodyConstraint
 {
 protected:
-
-
-	btScalar	m_desiredVelocity;
+	btScalar m_desiredVelocity;
+	btScalar m_desiredPosition;
+	btScalar m_kd;
+	btScalar m_kp;
+	btScalar m_erp;
+	btScalar m_rhsClamp;  //maximum error
 
 public:
-
 	btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
 	btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
 	virtual ~btMultiBodyJointMotor();
-    virtual void finalizeMultiDof();
+	virtual void finalizeMultiDof();
 
 	virtual int getIslandIdA() const;
 	virtual int getIslandIdB() const;
 
 	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal);
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f)
+	{
+		m_desiredVelocity = velTarget;
+		m_kd = kd;
+	}
 
-    virtual void setVelocityTarget(btScalar velTarget)
-    {
-        m_desiredVelocity = velTarget;
-    }
+	virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f)
+	{
+		m_desiredPosition = posTarget;
+		m_kp = kp;
+	}
 
+	virtual void setErp(btScalar erp)
+	{
+		m_erp = erp;
+	}
+	virtual btScalar getErp() const
+	{
+		return m_erp;
+	}
+	virtual void setRhsClamp(btScalar rhsClamp)
+	{
+		m_rhsClamp = rhsClamp;
+	}
 	virtual void debugDraw(class btIDebugDraw* drawer)
 	{
 		//todo(erwincoumans)
 	}
 };
 
-#endif //BT_MULTIBODY_JOINT_MOTOR_H
-
+#endif  //BT_MULTIBODY_JOINT_MOTOR_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h
index 668e4443904..01d5583c2fd 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h
@@ -20,9 +20,10 @@ subject to the following restrictions:
 #include "LinearMath/btVector3.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
-enum	btMultiBodyLinkFlags
+enum btMultiBodyLinkFlags
 {
-	BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1
+	BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1,
+	BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2,
 };
 
 //both defines are now permanently enabled
@@ -35,7 +36,6 @@ enum	btMultiBodyLinkFlags
 
 //namespace {
 
-
 #include "LinearMath/btSpatialAlgebra.h"
 
 //}
@@ -44,27 +44,26 @@ enum	btMultiBodyLinkFlags
 // Link struct
 //
 
-struct btMultibodyLink 
+struct btMultibodyLink
 {
-
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-    btScalar m_mass;         // mass of link
-    btVector3 m_inertiaLocal;   // inertia of link (local frame; diagonal)
+	btScalar m_mass;           // mass of link
+	btVector3 m_inertiaLocal;  // inertia of link (local frame; diagonal)
+
+	int m_parent;  // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
 
-    int m_parent;         // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
+	btQuaternion m_zeroRotParentToThis;  // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
 
-    btQuaternion m_zeroRotParentToThis;    // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
+	btVector3 m_dVector;  // vector from the inboard joint pos to this link's COM. (local frame.) constant.
+						  //this is set to zero for planar joint (see also m_eVector comment)
 
-    btVector3 m_dVector;   // vector from the inboard joint pos to this link's COM. (local frame.) constant.
-	//this is set to zero for planar joint (see also m_eVector comment)
-	
-    // m_eVector is constant, but depends on the joint type:
-    // revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
+	// m_eVector is constant, but depends on the joint type:
+	// revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
 	// planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
 	// todo: fix the planar so it is consistent with the other joints
-	
-    btVector3 m_eVector;
+
+	btVector3 m_eVector;
 
 	btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
 
@@ -78,13 +77,11 @@ struct btMultibodyLink
 		eInvalid
 	};
 
-	
-
 	// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
-    // for prismatic: m_axesTop[0] = zero;
-    //                m_axesBottom[0] = unit vector along the joint axis.
-    // for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
-    //               m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint)
+	// for prismatic: m_axesTop[0] = zero;
+	//                m_axesBottom[0] = unit vector along the joint axis.
+	// for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
+	//               m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint)
 	//
 	// for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes)
 	//				  m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint)
@@ -92,117 +89,149 @@ struct btMultibodyLink
 	// for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion
 	//			   m_axesTop[1][2] = zero
 	//			   m_axesBottom[0] = zero
-	//			   m_axesBottom[1][2] = unit vectors along the translational axes on that plane		
+	//			   m_axesBottom[1][2] = unit vectors along the translational axes on that plane
 	btSpatialMotionVector m_axes[6];
 	void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
-	void setAxisBottom(int dof, const btVector3 &axis) { m_axes[dof].m_bottomVec = axis; }
-	void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_topVec.setValue(x, y, z); }
-	void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_bottomVec.setValue(x, y, z); }
-	const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
-	const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
+	void setAxisBottom(int dof, const btVector3 &axis)
+	{
+		m_axes[dof].m_bottomVec = axis;
+	}
+	void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
+	{
+		m_axes[dof].m_topVec.setValue(x, y, z);
+	}
+	void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
+	{
+		m_axes[dof].m_bottomVec.setValue(x, y, z);
+	}
+	const btVector3 &getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
+	const btVector3 &getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
 
 	int m_dofOffset, m_cfgOffset;
 
-    btQuaternion m_cachedRotParentToThis;   // rotates vectors in parent frame to vectors in local frame
-    btVector3 m_cachedRVector;                // vector from COM of parent to COM of this link, in local frame.
+	btQuaternion m_cachedRotParentToThis;  // rotates vectors in parent frame to vectors in local frame
+	btVector3 m_cachedRVector;             // vector from COM of parent to COM of this link, in local frame.
+    
+    // predicted verstion
+    btQuaternion m_cachedRotParentToThis_interpolate;  // rotates vectors in parent frame to vectors in local frame
+    btVector3 m_cachedRVector_interpolate;             // vector from COM of parent to COM of this link, in local frame.
 
-    btVector3 m_appliedForce;    // In WORLD frame
-    btVector3 m_appliedTorque;   // In WORLD frame
+	btVector3 m_appliedForce;   // In WORLD frame
+	btVector3 m_appliedTorque;  // In WORLD frame
 
-btVector3 m_appliedConstraintForce;    // In WORLD frame
-    btVector3 m_appliedConstraintTorque;   // In WORLD frame
+	btVector3 m_appliedConstraintForce;   // In WORLD frame
+	btVector3 m_appliedConstraintTorque;  // In WORLD frame
 
 	btScalar m_jointPos[7];
-    
-    //m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
-    //It gets set to zero after each internal stepSimulation call
+    btScalar m_jointPos_interpolate[7];
+
+	//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
+	//It gets set to zero after each internal stepSimulation call
 	btScalar m_jointTorque[6];
-    
-	class btMultiBodyLinkCollider* m_collider;
+
+	class btMultiBodyLinkCollider *m_collider;
 	int m_flags;
-	
-	
-	int m_dofCount, m_posVarCount;				//redundant but handy
-	
+
+	int m_dofCount, m_posVarCount;  //redundant but handy
+
 	eFeatherstoneJointType m_jointType;
-	
-	struct btMultiBodyJointFeedback*	m_jointFeedback;
 
-	btTransform	m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics
+	struct btMultiBodyJointFeedback *m_jointFeedback;
 
-	const char* m_linkName;//m_linkName memory needs to be managed by the developer/user!
-	const char* m_jointName;//m_jointName memory needs to be managed by the developer/user!
+	btTransform m_cachedWorldTransform;  //this cache is updated when calling btMultiBody::forwardKinematics
 
-    // ctor: set some sensible defaults
+	const char *m_linkName;   //m_linkName memory needs to be managed by the developer/user!
+	const char *m_jointName;  //m_jointName memory needs to be managed by the developer/user!
+	const void *m_userPtr;    //m_userPtr ptr needs to be managed by the developer/user!
+
+	btScalar m_jointDamping;      //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
+	btScalar m_jointFriction;     //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
+	btScalar m_jointLowerLimit;   //todo: implement this internally. It is unused for now, it is set by a URDF loader.
+	btScalar m_jointUpperLimit;   //todo: implement this internally. It is unused for now, it is set by a URDF loader.
+	btScalar m_jointMaxForce;     //todo: implement this internally. It is unused for now, it is set by a URDF loader.
+	btScalar m_jointMaxVelocity;  //todo: implement this internally. It is unused for now, it is set by a URDF loader.
+
+	// ctor: set some sensible defaults
 	btMultibodyLink()
-		: 	m_mass(1),
-			m_parent(-1),
-			m_zeroRotParentToThis(0, 0, 0, 1),
-			m_cachedRotParentToThis(0, 0, 0, 1),
-			m_collider(0),
-			m_flags(0),
-			m_dofCount(0),
-			m_posVarCount(0),
-			m_jointType(btMultibodyLink::eInvalid),
-			m_jointFeedback(0),
-			m_linkName(0),
-			m_jointName(0)
+		: m_mass(1),
+		  m_parent(-1),
+		  m_zeroRotParentToThis(0, 0, 0, 1),
+		  m_cachedRotParentToThis(0, 0, 0, 1),
+          m_cachedRotParentToThis_interpolate(0, 0, 0, 1),
+		  m_collider(0),
+		  m_flags(0),
+		  m_dofCount(0),
+		  m_posVarCount(0),
+		  m_jointType(btMultibodyLink::eInvalid),
+		  m_jointFeedback(0),
+		  m_linkName(0),
+		  m_jointName(0),
+		  m_userPtr(0),
+		  m_jointDamping(0),
+		  m_jointFriction(0),
+		  m_jointLowerLimit(0),
+		  m_jointUpperLimit(0),
+		  m_jointMaxForce(0),
+		  m_jointMaxVelocity(0)
 	{
-		
 		m_inertiaLocal.setValue(1, 1, 1);
 		setAxisTop(0, 0., 0., 0.);
 		setAxisBottom(0, 1., 0., 0.);
 		m_dVector.setValue(0, 0, 0);
 		m_eVector.setValue(0, 0, 0);
 		m_cachedRVector.setValue(0, 0, 0);
-		m_appliedForce.setValue( 0, 0, 0);
+        m_cachedRVector_interpolate.setValue(0, 0, 0);
+		m_appliedForce.setValue(0, 0, 0);
 		m_appliedTorque.setValue(0, 0, 0);
-		//		
+		m_appliedConstraintForce.setValue(0, 0, 0);
+		m_appliedConstraintTorque.setValue(0, 0, 0);
+		//
 		m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
-		m_jointPos[3] = 1.f;			//"quat.w"
+		m_jointPos[3] = 1.f;  //"quat.w"
 		m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
 		m_cachedWorldTransform.setIdentity();
 	}
 
-    // routine to update m_cachedRotParentToThis and m_cachedRVector
+	// routine to update m_cachedRotParentToThis and m_cachedRVector
 	void updateCacheMultiDof(btScalar *pq = 0)
 	{
-		btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
-
-		switch(m_jointType)
+        btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
+        btQuaternion& cachedRot = m_cachedRotParentToThis;
+        btVector3& cachedVector = m_cachedRVector;
+		switch (m_jointType)
 		{
 			case eRevolute:
 			{
-				m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
+				cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
 
 				break;
 			}
 			case ePrismatic:
 			{
 				// m_cachedRotParentToThis never changes, so no need to update
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);
+				cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
 
 				break;
 			}
 			case eSpherical:
 			{
-				m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
+				cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
 
 				break;
 			}
 			case ePlanar:
 			{
-				m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;				
-				m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector);				
+				cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+				cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
 
 				break;
 			}
 			case eFixed:
 			{
-				m_cachedRotParentToThis = m_zeroRotParentToThis;
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
+				cachedRot = m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
 
 				break;
 			}
@@ -212,8 +241,60 @@ btVector3 m_appliedConstraintForce;    // In WORLD frame
 				btAssert(0);
 			}
 		}
+        m_cachedRotParentToThis_interpolate = m_cachedRotParentToThis;
+        m_cachedRVector_interpolate = m_cachedRVector;
 	}
+    
+    void updateInterpolationCacheMultiDof()
+    {
+        btScalar *pJointPos = &m_jointPos_interpolate[0];
+        
+        btQuaternion& cachedRot = m_cachedRotParentToThis_interpolate;
+        btVector3& cachedVector = m_cachedRVector_interpolate;
+        switch (m_jointType)
+        {
+            case eRevolute:
+            {
+                cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+                cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
+                
+                break;
+            }
+            case ePrismatic:
+            {
+                // m_cachedRotParentToThis never changes, so no need to update
+                cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
+                
+                break;
+            }
+            case eSpherical:
+            {
+                cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
+                cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
+                
+                break;
+            }
+            case ePlanar:
+            {
+                cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+                cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
+                
+                break;
+            }
+            case eFixed:
+            {
+                cachedRot = m_zeroRotParentToThis;
+                cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
+                
+                break;
+            }
+            default:
+            {
+                //invalid type
+                btAssert(0);
+            }
+        }
+    }
 };
 
-
-#endif //BT_MULTIBODY_LINK_H
+#endif  //BT_MULTIBODY_LINK_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
index 5080ea87454..bc909990c21 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
@@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 
 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -19,21 +19,32 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
 #include "btMultiBody.h"
+#include "LinearMath/btSerializer.h"
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btMultiBodyLinkColliderData btMultiBodyLinkColliderDoubleData
+#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderDoubleData"
+#else
+#define btMultiBodyLinkColliderData btMultiBodyLinkColliderFloatData
+#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderFloatData"
+#endif
 
 class btMultiBodyLinkCollider : public btCollisionObject
 {
-//protected:
+	//protected:
 public:
-
 	btMultiBody* m_multiBody;
 	int m_link;
 
+	virtual ~btMultiBodyLinkCollider()
+	{
 
-	btMultiBodyLinkCollider (btMultiBody* multiBody,int link)
-		:m_multiBody(multiBody),
-		m_link(link)
+	}
+	btMultiBodyLinkCollider(btMultiBody* multiBody, int link)
+		: m_multiBody(multiBody),
+		  m_link(link)
 	{
-		m_checkCollideWith =  true;
+		m_checkCollideWith = true;
 		//we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands
 		//this means that some constraints might point to bodies that are not in the islands, causing crashes
 		//if (link>=0 || (multiBody && !multiBody->hasFixedBase()))
@@ -49,18 +60,18 @@ public:
 	}
 	static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj)
 	{
-		if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
+		if (colObj->getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK)
 			return (btMultiBodyLinkCollider*)colObj;
 		return 0;
 	}
 	static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj)
 	{
-		if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK)
+		if (colObj->getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK)
 			return (btMultiBodyLinkCollider*)colObj;
 		return 0;
 	}
 
-	virtual bool checkCollideWithOverride(const  btCollisionObject* co) const
+	virtual bool checkCollideWithOverride(const btCollisionObject* co) const
 	{
 		const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co);
 		if (!other)
@@ -71,22 +82,97 @@ public:
 			return false;
 
 		//check if 'link' has collision disabled
-		if (m_link>=0)
+		if (m_link >= 0)
 		{
 			const btMultibodyLink& link = m_multiBody->getLink(this->m_link);
-			if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link)
-				return false;
+			if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION)
+			{
+				int parent_of_this = m_link;
+				while (1)
+				{
+					if (parent_of_this == -1)
+						break;
+					parent_of_this = m_multiBody->getLink(parent_of_this).m_parent;
+					if (parent_of_this == other->m_link)
+					{
+						return false;
+					}
+				}
+			}
+			else if (link.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
+			{
+				if (link.m_parent == other->m_link)
+					return false;
+			}
 		}
-		
-		if (other->m_link>=0)
+
+		if (other->m_link >= 0)
 		{
 			const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link);
-			if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link)
-				return false;
+			if (otherLink.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION)
+			{
+				int parent_of_other = other->m_link;
+				while (1)
+				{
+					if (parent_of_other == -1)
+						break;
+					parent_of_other = m_multiBody->getLink(parent_of_other).m_parent;
+					if (parent_of_other == this->m_link)
+						return false;
+				}
+			}
+			else if (otherLink.m_flags & BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION)
+			{
+				if (otherLink.m_parent == this->m_link)
+					return false;
+			}
 		}
 		return true;
 	}
+
+	virtual int calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
+};
+
+// clang-format off
+
+struct	btMultiBodyLinkColliderFloatData
+{
+	btCollisionObjectFloatData m_colObjData;
+	btMultiBodyFloatData	*m_multiBody;
+	int			m_link;
+	char		m_padding[4];
 };
 
-#endif //BT_FEATHERSTONE_LINK_COLLIDER_H
+struct	btMultiBodyLinkColliderDoubleData
+{
+	btCollisionObjectDoubleData m_colObjData;
+	btMultiBodyDoubleData		*m_multiBody;
+	int			m_link;
+	char		m_padding[4];
+};
+
+// clang-format on
+
+SIMD_FORCE_INLINE int btMultiBodyLinkCollider::calculateSerializeBufferSize() const
+{
+	return sizeof(btMultiBodyLinkColliderData);
+}
+
+SIMD_FORCE_INLINE const char* btMultiBodyLinkCollider::serialize(void* dataBuffer, class btSerializer* serializer) const
+{
+	btMultiBodyLinkColliderData* dataOut = (btMultiBodyLinkColliderData*)dataBuffer;
+	btCollisionObject::serialize(&dataOut->m_colObjData, serializer);
+
+	dataOut->m_link = this->m_link;
+	dataOut->m_multiBody = (btMultiBodyData*)serializer->getUniquePointer(m_multiBody);
+
+	// Fill padding with zeros to appease msan.
+	memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding));
+
+	return btMultiBodyLinkColliderDataName;
+}
 
+#endif  //BT_FEATHERSTONE_LINK_COLLIDER_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp
new file mode 100644
index 00000000000..f2186a93e9d
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp
@@ -0,0 +1,966 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
+
+#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+
+static bool interleaveContactAndFriction1 = false;
+
+struct btJointNode1
+{
+	int jointIndex;          // pointer to enclosing dxJoint object
+	int otherBodyIndex;      // *other* body this joint is connected to
+	int nextJointNodeIndex;  //-1 for null
+	int constraintRowIndex;
+};
+
+// Helper function to compute a delta velocity in the constraint space.
+static btScalar computeDeltaVelocityInConstraintSpace(
+	const btVector3& angularDeltaVelocity,
+	const btVector3& contactNormal,
+	btScalar invMass,
+	const btVector3& angularJacobian,
+	const btVector3& linearJacobian)
+{
+	return angularDeltaVelocity.dot(angularJacobian) + contactNormal.dot(linearJacobian) * invMass;
+}
+
+// Faster version of computeDeltaVelocityInConstraintSpace that can be used when contactNormal and linearJacobian are
+// identical.
+static btScalar computeDeltaVelocityInConstraintSpace(
+	const btVector3& angularDeltaVelocity,
+	btScalar invMass,
+	const btVector3& angularJacobian)
+{
+	return angularDeltaVelocity.dot(angularJacobian) + invMass;
+}
+
+// Helper function to compute a delta velocity in the constraint space.
+static btScalar computeDeltaVelocityInConstraintSpace(const btScalar* deltaVelocity, const btScalar* jacobian, int size)
+{
+	btScalar result = 0;
+	for (int i = 0; i < size; ++i)
+		result += deltaVelocity[i] * jacobian[i];
+
+	return result;
+}
+
+static btScalar computeConstraintMatrixDiagElementMultiBody(
+	const btAlignedObjectArray<btSolverBody>& solverBodyPool,
+	const btMultiBodyJacobianData& data,
+	const btMultiBodySolverConstraint& constraint)
+{
+	btScalar ret = 0;
+
+	const btMultiBody* multiBodyA = constraint.m_multiBodyA;
+	const btMultiBody* multiBodyB = constraint.m_multiBodyB;
+
+	if (multiBodyA)
+	{
+		const btScalar* jacA = &data.m_jacobians[constraint.m_jacAindex];
+		const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
+		const int ndofA = multiBodyA->getNumDofs() + 6;
+		ret += computeDeltaVelocityInConstraintSpace(deltaA, jacA, ndofA);
+	}
+	else
+	{
+		const int solverBodyIdA = constraint.m_solverBodyIdA;
+		btAssert(solverBodyIdA != -1);
+		const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
+		const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
+		ret += computeDeltaVelocityInConstraintSpace(
+			constraint.m_relpos1CrossNormal,
+			invMassA,
+			constraint.m_angularComponentA);
+	}
+
+	if (multiBodyB)
+	{
+		const btScalar* jacB = &data.m_jacobians[constraint.m_jacBindex];
+		const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
+		const int ndofB = multiBodyB->getNumDofs() + 6;
+		ret += computeDeltaVelocityInConstraintSpace(deltaB, jacB, ndofB);
+	}
+	else
+	{
+		const int solverBodyIdB = constraint.m_solverBodyIdB;
+		btAssert(solverBodyIdB != -1);
+		const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
+		const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
+		ret += computeDeltaVelocityInConstraintSpace(
+			constraint.m_relpos2CrossNormal,
+			invMassB,
+			constraint.m_angularComponentB);
+	}
+
+	return ret;
+}
+
+static btScalar computeConstraintMatrixOffDiagElementMultiBody(
+	const btAlignedObjectArray<btSolverBody>& solverBodyPool,
+	const btMultiBodyJacobianData& data,
+	const btMultiBodySolverConstraint& constraint,
+	const btMultiBodySolverConstraint& offDiagConstraint)
+{
+	btScalar offDiagA = btScalar(0);
+
+	const btMultiBody* multiBodyA = constraint.m_multiBodyA;
+	const btMultiBody* multiBodyB = constraint.m_multiBodyB;
+	const btMultiBody* offDiagMultiBodyA = offDiagConstraint.m_multiBodyA;
+	const btMultiBody* offDiagMultiBodyB = offDiagConstraint.m_multiBodyB;
+
+	// Assumed at least one system is multibody
+	btAssert(multiBodyA || multiBodyB);
+	btAssert(offDiagMultiBodyA || offDiagMultiBodyB);
+
+	if (offDiagMultiBodyA)
+	{
+		const btScalar* offDiagJacA = &data.m_jacobians[offDiagConstraint.m_jacAindex];
+
+		if (offDiagMultiBodyA == multiBodyA)
+		{
+			const int ndofA = multiBodyA->getNumDofs() + 6;
+			const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacA, ndofA);
+		}
+		else if (offDiagMultiBodyA == multiBodyB)
+		{
+			const int ndofB = multiBodyB->getNumDofs() + 6;
+			const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacA, ndofB);
+		}
+	}
+	else
+	{
+		const int solverBodyIdA = constraint.m_solverBodyIdA;
+		const int solverBodyIdB = constraint.m_solverBodyIdB;
+
+		const int offDiagSolverBodyIdA = offDiagConstraint.m_solverBodyIdA;
+		btAssert(offDiagSolverBodyIdA != -1);
+
+		if (offDiagSolverBodyIdA == solverBodyIdA)
+		{
+			btAssert(solverBodyIdA != -1);
+			const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
+			const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos1CrossNormal,
+				offDiagConstraint.m_contactNormal1,
+				invMassA, constraint.m_angularComponentA,
+				constraint.m_contactNormal1);
+		}
+		else if (offDiagSolverBodyIdA == solverBodyIdB)
+		{
+			btAssert(solverBodyIdB != -1);
+			const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
+			const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos1CrossNormal,
+				offDiagConstraint.m_contactNormal1,
+				invMassB,
+				constraint.m_angularComponentB,
+				constraint.m_contactNormal2);
+		}
+	}
+
+	if (offDiagMultiBodyB)
+	{
+		const btScalar* offDiagJacB = &data.m_jacobians[offDiagConstraint.m_jacBindex];
+
+		if (offDiagMultiBodyB == multiBodyA)
+		{
+			const int ndofA = multiBodyA->getNumDofs() + 6;
+			const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacB, ndofA);
+		}
+		else if (offDiagMultiBodyB == multiBodyB)
+		{
+			const int ndofB = multiBodyB->getNumDofs() + 6;
+			const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
+			offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacB, ndofB);
+		}
+	}
+	else
+	{
+		const int solverBodyIdA = constraint.m_solverBodyIdA;
+		const int solverBodyIdB = constraint.m_solverBodyIdB;
+
+		const int offDiagSolverBodyIdB = offDiagConstraint.m_solverBodyIdB;
+		btAssert(offDiagSolverBodyIdB != -1);
+
+		if (offDiagSolverBodyIdB == solverBodyIdA)
+		{
+			btAssert(solverBodyIdA != -1);
+			const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
+			const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos2CrossNormal,
+				offDiagConstraint.m_contactNormal2,
+				invMassA, constraint.m_angularComponentA,
+				constraint.m_contactNormal1);
+		}
+		else if (offDiagSolverBodyIdB == solverBodyIdB)
+		{
+			btAssert(solverBodyIdB != -1);
+			const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
+			const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
+			offDiagA += computeDeltaVelocityInConstraintSpace(
+				offDiagConstraint.m_relpos2CrossNormal,
+				offDiagConstraint.m_contactNormal2,
+				invMassB, constraint.m_angularComponentB,
+				constraint.m_contactNormal2);
+		}
+	}
+
+	return offDiagA;
+}
+
+void btMultiBodyMLCPConstraintSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
+{
+	createMLCPFastRigidBody(infoGlobal);
+	createMLCPFastMultiBody(infoGlobal);
+}
+
+void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal)
+{
+	int numContactRows = interleaveContactAndFriction1 ? 3 : 1;
+
+	int numConstraintRows = m_allConstraintPtrArray.size();
+
+	if (numConstraintRows == 0)
+		return;
+
+	int n = numConstraintRows;
+	{
+		BT_PROFILE("init b (rhs)");
+		m_b.resize(numConstraintRows);
+		m_bSplit.resize(numConstraintRows);
+		m_b.setZero();
+		m_bSplit.setZero();
+		for (int i = 0; i < numConstraintRows; i++)
+		{
+			btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
+			if (!btFuzzyZero(jacDiag))
+			{
+				btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
+				btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
+				m_b[i] = rhs / jacDiag;
+				m_bSplit[i] = rhsPenetration / jacDiag;
+			}
+		}
+	}
+
+	//	btScalar* w = 0;
+	//	int nub = 0;
+
+	m_lo.resize(numConstraintRows);
+	m_hi.resize(numConstraintRows);
+
+	{
+		BT_PROFILE("init lo/ho");
+
+		for (int i = 0; i < numConstraintRows; i++)
+		{
+			if (0)  //m_limitDependencies[i]>=0)
+			{
+				m_lo[i] = -BT_INFINITY;
+				m_hi[i] = BT_INFINITY;
+			}
+			else
+			{
+				m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
+				m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
+			}
+		}
+	}
+
+	//
+	int m = m_allConstraintPtrArray.size();
+
+	int numBodies = m_tmpSolverBodyPool.size();
+	btAlignedObjectArray<int> bodyJointNodeArray;
+	{
+		BT_PROFILE("bodyJointNodeArray.resize");
+		bodyJointNodeArray.resize(numBodies, -1);
+	}
+	btAlignedObjectArray<btJointNode1> jointNodeArray;
+	{
+		BT_PROFILE("jointNodeArray.reserve");
+		jointNodeArray.reserve(2 * m_allConstraintPtrArray.size());
+	}
+
+	btMatrixXu& J3 = m_scratchJ3;
+	{
+		BT_PROFILE("J3.resize");
+		J3.resize(2 * m, 8);
+	}
+	btMatrixXu& JinvM3 = m_scratchJInvM3;
+	{
+		BT_PROFILE("JinvM3.resize/setZero");
+
+		JinvM3.resize(2 * m, 8);
+		JinvM3.setZero();
+		J3.setZero();
+	}
+	int cur = 0;
+	int rowOffset = 0;
+	btAlignedObjectArray<int>& ofs = m_scratchOfs;
+	{
+		BT_PROFILE("ofs resize");
+		ofs.resize(0);
+		ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
+	}
+	{
+		BT_PROFILE("Compute J and JinvM");
+		int c = 0;
+
+		int numRows = 0;
+
+		for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
+		{
+			ofs[c] = rowOffset;
+			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
+			int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
+			btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+			btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+			numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
+			if (orgBodyA)
+			{
+				{
+					int slotA = -1;
+					//find free jointNode slot for sbA
+					slotA = jointNodeArray.size();
+					jointNodeArray.expand();  //NonInitializing();
+					int prevSlot = bodyJointNodeArray[sbA];
+					bodyJointNodeArray[sbA] = slotA;
+					jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
+					jointNodeArray[slotA].jointIndex = c;
+					jointNodeArray[slotA].constraintRowIndex = i;
+					jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
+				}
+				for (int row = 0; row < numRows; row++, cur++)
+				{
+					btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass();
+					btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld();
+
+					for (int r = 0; r < 3; r++)
+					{
+						J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]);
+						J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]);
+						JinvM3.setElem(cur, r, normalInvMass[r]);
+						JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]);
+					}
+					J3.setElem(cur, 3, 0);
+					JinvM3.setElem(cur, 3, 0);
+					J3.setElem(cur, 7, 0);
+					JinvM3.setElem(cur, 7, 0);
+				}
+			}
+			else
+			{
+				cur += numRows;
+			}
+			if (orgBodyB)
+			{
+				{
+					int slotB = -1;
+					//find free jointNode slot for sbA
+					slotB = jointNodeArray.size();
+					jointNodeArray.expand();  //NonInitializing();
+					int prevSlot = bodyJointNodeArray[sbB];
+					bodyJointNodeArray[sbB] = slotB;
+					jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
+					jointNodeArray[slotB].jointIndex = c;
+					jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
+					jointNodeArray[slotB].constraintRowIndex = i;
+				}
+
+				for (int row = 0; row < numRows; row++, cur++)
+				{
+					btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass();
+					btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
+
+					for (int r = 0; r < 3; r++)
+					{
+						J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]);
+						J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]);
+						JinvM3.setElem(cur, r, normalInvMassB[r]);
+						JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]);
+					}
+					J3.setElem(cur, 3, 0);
+					JinvM3.setElem(cur, 3, 0);
+					J3.setElem(cur, 7, 0);
+					JinvM3.setElem(cur, 7, 0);
+				}
+			}
+			else
+			{
+				cur += numRows;
+			}
+			rowOffset += numRows;
+		}
+	}
+
+	//compute JinvM = J*invM.
+	const btScalar* JinvM = JinvM3.getBufferPointer();
+
+	const btScalar* Jptr = J3.getBufferPointer();
+	{
+		BT_PROFILE("m_A.resize");
+		m_A.resize(n, n);
+	}
+
+	{
+		BT_PROFILE("m_A.setZero");
+		m_A.setZero();
+	}
+	int c = 0;
+	{
+		int numRows = 0;
+		BT_PROFILE("Compute A");
+		for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
+		{
+			int row__ = ofs[c];
+			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
+			int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
+			//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+			//	btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+			numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
+
+			const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
+
+			{
+				int startJointNodeA = bodyJointNodeArray[sbA];
+				while (startJointNodeA >= 0)
+				{
+					int j0 = jointNodeArray[startJointNodeA].jointIndex;
+					int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
+					if (j0 < c)
+					{
+						int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
+						size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0;
+						//printf("%d joint i %d and j0: %d: ",count++,i,j0);
+						m_A.multiplyAdd2_p8r(JinvMrow,
+											 Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
+					}
+					startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
+				}
+			}
+
+			{
+				int startJointNodeB = bodyJointNodeArray[sbB];
+				while (startJointNodeB >= 0)
+				{
+					int j1 = jointNodeArray[startJointNodeB].jointIndex;
+					int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
+
+					if (j1 < c)
+					{
+						int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
+						size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0;
+						m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows,
+											 Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
+					}
+					startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
+				}
+			}
+		}
+
+		{
+			BT_PROFILE("compute diagonal");
+			// compute diagonal blocks of m_A
+
+			int row__ = 0;
+			int numJointRows = m_allConstraintPtrArray.size();
+
+			int jj = 0;
+			for (; row__ < numJointRows;)
+			{
+				//int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
+				int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
+				//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+				btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+				const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
+
+				const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
+				const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__;
+				m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__);
+				if (orgBodyB)
+				{
+					m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__);
+				}
+				row__ += infom;
+				jj++;
+			}
+		}
+	}
+
+	if (1)
+	{
+		// add cfm to the diagonal of m_A
+		for (int i = 0; i < m_A.rows(); ++i)
+		{
+			m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
+		}
+	}
+
+	///fill the upper triangle of the matrix, to make it symmetric
+	{
+		BT_PROFILE("fill the upper triangle ");
+		m_A.copyLowerToUpperTriangle();
+	}
+
+	{
+		BT_PROFILE("resize/init x");
+		m_x.resize(numConstraintRows);
+		m_xSplit.resize(numConstraintRows);
+
+		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+		{
+			for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
+			{
+				const btSolverConstraint& c = *m_allConstraintPtrArray[i];
+				m_x[i] = c.m_appliedImpulse;
+				m_xSplit[i] = c.m_appliedPushImpulse;
+			}
+		}
+		else
+		{
+			m_x.setZero();
+			m_xSplit.setZero();
+		}
+	}
+}
+
+void btMultiBodyMLCPConstraintSolver::createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal)
+{
+	const int multiBodyNumConstraints = m_multiBodyAllConstraintPtrArray.size();
+
+	if (multiBodyNumConstraints == 0)
+		return;
+
+	// 1. Compute b
+	{
+		BT_PROFILE("init b (rhs)");
+
+		m_multiBodyB.resize(multiBodyNumConstraints);
+		m_multiBodyB.setZero();
+
+		for (int i = 0; i < multiBodyNumConstraints; ++i)
+		{
+			const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+			const btScalar jacDiag = constraint.m_jacDiagABInv;
+
+			if (!btFuzzyZero(jacDiag))
+			{
+				// Note that rhsPenetration is currently always zero because the split impulse hasn't been implemented for multibody yet.
+				const btScalar rhs = constraint.m_rhs;
+				m_multiBodyB[i] = rhs / jacDiag;
+			}
+		}
+	}
+
+	// 2. Compute lo and hi
+	{
+		BT_PROFILE("init lo/ho");
+
+		m_multiBodyLo.resize(multiBodyNumConstraints);
+		m_multiBodyHi.resize(multiBodyNumConstraints);
+
+		for (int i = 0; i < multiBodyNumConstraints; ++i)
+		{
+			const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+			m_multiBodyLo[i] = constraint.m_lowerLimit;
+			m_multiBodyHi[i] = constraint.m_upperLimit;
+		}
+	}
+
+	// 3. Construct A matrix by using the impulse testing
+	{
+		BT_PROFILE("Compute A");
+
+		{
+			BT_PROFILE("m_A.resize");
+			m_multiBodyA.resize(multiBodyNumConstraints, multiBodyNumConstraints);
+		}
+
+		for (int i = 0; i < multiBodyNumConstraints; ++i)
+		{
+			// Compute the diagonal of A, which is A(i, i)
+			const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+			const btScalar diagA = computeConstraintMatrixDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint);
+			m_multiBodyA.setElem(i, i, diagA);
+
+			// Computes the off-diagonals of A:
+			//   a. The rest of i-th row of A, from A(i, i+1) to A(i, n)
+			//   b. The rest of i-th column of A, from A(i+1, i) to A(n, i)
+			for (int j = i + 1; j < multiBodyNumConstraints; ++j)
+			{
+				const btMultiBodySolverConstraint& offDiagConstraint = *m_multiBodyAllConstraintPtrArray[j];
+				const btScalar offDiagA = computeConstraintMatrixOffDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint, offDiagConstraint);
+
+				// Set the off-diagonal values of A. Note that A is symmetric.
+				m_multiBodyA.setElem(i, j, offDiagA);
+				m_multiBodyA.setElem(j, i, offDiagA);
+			}
+		}
+	}
+
+	// Add CFM to the diagonal of m_A
+	for (int i = 0; i < m_multiBodyA.rows(); ++i)
+	{
+		m_multiBodyA.setElem(i, i, m_multiBodyA(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
+	}
+
+	// 4. Initialize x
+	{
+		BT_PROFILE("resize/init x");
+
+		m_multiBodyX.resize(multiBodyNumConstraints);
+
+		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+		{
+			for (int i = 0; i < multiBodyNumConstraints; ++i)
+			{
+				const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
+				m_multiBodyX[i] = constraint.m_appliedImpulse;
+			}
+		}
+		else
+		{
+			m_multiBodyX.setZero();
+		}
+	}
+}
+
+bool btMultiBodyMLCPConstraintSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
+{
+	bool result = true;
+
+	if (m_A.rows() != 0)
+	{
+		// If using split impulse, we solve 2 separate (M)LCPs
+		if (infoGlobal.m_splitImpulse)
+		{
+			const btMatrixXu Acopy = m_A;
+			const btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
+			// TODO(JS): Do we really need these copies when solveMLCP takes them as const?
+
+			result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
+			if (result)
+				result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations);
+		}
+		else
+		{
+			result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
+		}
+	}
+
+	if (!result)
+		return false;
+
+	if (m_multiBodyA.rows() != 0)
+	{
+		result = m_solver->solveMLCP(m_multiBodyA, m_multiBodyB, m_multiBodyX, m_multiBodyLo, m_multiBodyHi, m_multiBodyLimitDependencies, infoGlobal.m_numIterations);
+	}
+
+	return result;
+}
+
+btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup(
+	btCollisionObject** bodies,
+	int numBodies,
+	btPersistentManifold** manifoldPtr,
+	int numManifolds,
+	btTypedConstraint** constraints,
+	int numConstraints,
+	const btContactSolverInfo& infoGlobal,
+	btIDebugDraw* debugDrawer)
+{
+	// 1. Setup for rigid-bodies
+	btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(
+		bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+
+	// 2. Setup for multi-bodies
+	//   a. Collect all different kinds of constraint as pointers into one array, m_allConstraintPtrArray
+	//   b. Set the index array for frictional contact constraints, m_limitDependencies
+	{
+		BT_PROFILE("gather constraint data");
+
+		int dindex = 0;
+
+		const int numRigidBodyConstraints = m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size();
+		const int numMultiBodyConstraints = m_multiBodyNonContactConstraints.size() + m_multiBodyNormalContactConstraints.size() + m_multiBodyFrictionContactConstraints.size();
+
+		m_allConstraintPtrArray.resize(0);
+		m_multiBodyAllConstraintPtrArray.resize(0);
+
+		// i. Setup for rigid bodies
+
+		m_limitDependencies.resize(numRigidBodyConstraints);
+
+		for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); ++i)
+		{
+			m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
+			m_limitDependencies[dindex++] = -1;
+		}
+
+		int firstContactConstraintOffset = dindex;
+
+		// The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
+		if (interleaveContactAndFriction1)
+		{
+			for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
+			{
+				const int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2;
+
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
+				m_limitDependencies[dindex++] = -1;
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]);
+				int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact));
+				m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
+				if (numFrictionPerContact == 2)
+				{
+					m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]);
+					m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
+				}
+			}
+		}
+		else
+		{
+			for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
+			{
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
+				m_limitDependencies[dindex++] = -1;
+			}
+			for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++)
+			{
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
+				m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset;
+			}
+		}
+
+		if (!m_allConstraintPtrArray.size())
+		{
+			m_A.resize(0, 0);
+			m_b.resize(0);
+			m_x.resize(0);
+			m_lo.resize(0);
+			m_hi.resize(0);
+		}
+
+		// ii. Setup for multibodies
+
+		dindex = 0;
+
+		m_multiBodyLimitDependencies.resize(numMultiBodyConstraints);
+
+		for (int i = 0; i < m_multiBodyNonContactConstraints.size(); ++i)
+		{
+			m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNonContactConstraints[i]);
+			m_multiBodyLimitDependencies[dindex++] = -1;
+		}
+
+		firstContactConstraintOffset = dindex;
+
+		// The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
+		if (interleaveContactAndFriction1)
+		{
+			for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
+			{
+				const int numtiBodyNumFrictionPerContact = m_multiBodyNormalContactConstraints.size() == m_multiBodyFrictionContactConstraints.size() ? 1 : 2;
+
+				m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
+				m_multiBodyLimitDependencies[dindex++] = -1;
+
+				btMultiBodySolverConstraint& frictionContactConstraint1 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact];
+				m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint1);
+
+				const int findex = (frictionContactConstraint1.m_frictionIndex * (1 + numtiBodyNumFrictionPerContact)) + firstContactConstraintOffset;
+
+				m_multiBodyLimitDependencies[dindex++] = findex;
+
+				if (numtiBodyNumFrictionPerContact == 2)
+				{
+					btMultiBodySolverConstraint& frictionContactConstraint2 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact + 1];
+					m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint2);
+
+					m_multiBodyLimitDependencies[dindex++] = findex;
+				}
+			}
+		}
+		else
+		{
+			for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
+			{
+				m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
+				m_multiBodyLimitDependencies[dindex++] = -1;
+			}
+			for (int i = 0; i < m_multiBodyFrictionContactConstraints.size(); ++i)
+			{
+				m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyFrictionContactConstraints[i]);
+				m_multiBodyLimitDependencies[dindex++] = m_multiBodyFrictionContactConstraints[i].m_frictionIndex + firstContactConstraintOffset;
+			}
+		}
+
+		if (!m_multiBodyAllConstraintPtrArray.size())
+		{
+			m_multiBodyA.resize(0, 0);
+			m_multiBodyB.resize(0);
+			m_multiBodyX.resize(0);
+			m_multiBodyLo.resize(0);
+			m_multiBodyHi.resize(0);
+		}
+	}
+
+	// Construct MLCP terms
+	{
+		BT_PROFILE("createMLCPFast");
+		createMLCPFast(infoGlobal);
+	}
+
+	return btScalar(0);
+}
+
+btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+{
+	bool result = true;
+	{
+		BT_PROFILE("solveMLCP");
+		result = solveMLCP(infoGlobal);
+	}
+
+	// Fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations if the solution isn't valid.
+	if (!result)
+	{
+		m_fallback++;
+		return btMultiBodyConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+	}
+
+	{
+		BT_PROFILE("process MLCP results");
+
+		for (int i = 0; i < m_allConstraintPtrArray.size(); ++i)
+		{
+			const btSolverConstraint& c = *m_allConstraintPtrArray[i];
+
+			const btScalar deltaImpulse = m_x[i] - c.m_appliedImpulse;
+			c.m_appliedImpulse = m_x[i];
+
+			int sbA = c.m_solverBodyIdA;
+			int sbB = c.m_solverBodyIdB;
+
+			btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
+			btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
+
+			solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+			solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+
+			if (infoGlobal.m_splitImpulse)
+			{
+				const btScalar deltaPushImpulse = m_xSplit[i] - c.m_appliedPushImpulse;
+				solverBodyA.internalApplyPushImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaPushImpulse);
+				solverBodyB.internalApplyPushImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaPushImpulse);
+				c.m_appliedPushImpulse = m_xSplit[i];
+			}
+		}
+
+		for (int i = 0; i < m_multiBodyAllConstraintPtrArray.size(); ++i)
+		{
+			btMultiBodySolverConstraint& c = *m_multiBodyAllConstraintPtrArray[i];
+
+			const btScalar deltaImpulse = m_multiBodyX[i] - c.m_appliedImpulse;
+			c.m_appliedImpulse = m_multiBodyX[i];
+
+			btMultiBody* multiBodyA = c.m_multiBodyA;
+			if (multiBodyA)
+			{
+				const int ndofA = multiBodyA->getNumDofs() + 6;
+				applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+				//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+				//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+				multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse);
+#endif  // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+			}
+			else
+			{
+				const int sbA = c.m_solverBodyIdA;
+				btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
+				solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+			}
+
+			btMultiBody* multiBodyB = c.m_multiBodyB;
+			if (multiBodyB)
+			{
+				const int ndofB = multiBodyB->getNumDofs() + 6;
+				applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB);
+#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+				//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
+				//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
+				multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse);
+#endif  // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
+			}
+			else
+			{
+				const int sbB = c.m_solverBodyIdB;
+				btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
+				solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+			}
+		}
+	}
+
+	return btScalar(0);
+}
+
+btMultiBodyMLCPConstraintSolver::btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver)
+	: m_solver(solver), m_fallback(0)
+{
+	// Do nothing
+}
+
+btMultiBodyMLCPConstraintSolver::~btMultiBodyMLCPConstraintSolver()
+{
+	// Do nothing
+}
+
+void btMultiBodyMLCPConstraintSolver::setMLCPSolver(btMLCPSolverInterface* solver)
+{
+	m_solver = solver;
+}
+
+int btMultiBodyMLCPConstraintSolver::getNumFallbacks() const
+{
+	return m_fallback;
+}
+
+void btMultiBodyMLCPConstraintSolver::setNumFallbacks(int num)
+{
+	m_fallback = num;
+}
+
+btConstraintSolverType btMultiBodyMLCPConstraintSolver::getSolverType() const
+{
+	return BT_MLCP_SOLVER;
+}
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h
new file mode 100644
index 00000000000..77fdb86bb95
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h
@@ -0,0 +1,187 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H
+#define BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H
+
+#include "LinearMath/btMatrixX.h"
+#include "LinearMath/btThreads.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+
+class btMLCPSolverInterface;
+class btMultiBody;
+
+class btMultiBodyMLCPConstraintSolver : public btMultiBodyConstraintSolver
+{
+protected:
+	/// \name MLCP Formulation for Rigid Bodies
+	/// \{
+
+	/// A matrix in the MLCP formulation
+	btMatrixXu m_A;
+
+	/// b vector in the MLCP formulation.
+	btVectorXu m_b;
+
+	/// Constraint impulse, which is an output of MLCP solving.
+	btVectorXu m_x;
+
+	/// Lower bound of constraint impulse, \c m_x.
+	btVectorXu m_lo;
+
+	/// Upper bound of constraint impulse, \c m_x.
+	btVectorXu m_hi;
+
+	/// \}
+
+	/// \name Cache Variables for Split Impulse for Rigid Bodies
+	/// When using 'split impulse' we solve two separate (M)LCPs
+	/// \{
+
+	/// Split impulse Cache vector corresponding to \c m_b.
+	btVectorXu m_bSplit;
+
+	/// Split impulse cache vector corresponding to \c m_x.
+	btVectorXu m_xSplit;
+
+	/// \}
+
+	/// \name MLCP Formulation for Multibodies
+	/// \{
+
+	/// A matrix in the MLCP formulation
+	btMatrixXu m_multiBodyA;
+
+	/// b vector in the MLCP formulation.
+	btVectorXu m_multiBodyB;
+
+	/// Constraint impulse, which is an output of MLCP solving.
+	btVectorXu m_multiBodyX;
+
+	/// Lower bound of constraint impulse, \c m_x.
+	btVectorXu m_multiBodyLo;
+
+	/// Upper bound of constraint impulse, \c m_x.
+	btVectorXu m_multiBodyHi;
+
+	/// \}
+
+	/// Indices of normal contact constraint associated with frictional contact constraint for rigid bodies.
+	///
+	/// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate
+	/// normal contact impulse. For example, i-th element represents the index of a normal constraint that is
+	/// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint.
+	/// Otherwise, -1.
+	btAlignedObjectArray<int> m_limitDependencies;
+
+	/// Indices of normal contact constraint associated with frictional contact constraint for multibodies.
+	///
+	/// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate
+	/// normal contact impulse. For example, i-th element represents the index of a normal constraint that is
+	/// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint.
+	/// Otherwise, -1.
+	btAlignedObjectArray<int> m_multiBodyLimitDependencies;
+
+	/// Array of all the rigid body constraints
+	btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray;
+
+	/// Array of all the multibody constraints
+	btAlignedObjectArray<btMultiBodySolverConstraint*> m_multiBodyAllConstraintPtrArray;
+
+	/// MLCP solver
+	btMLCPSolverInterface* m_solver;
+
+	/// Count of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP solver fails.
+	int m_fallback;
+
+	/// \name MLCP Scratch Variables
+	/// The following scratch variables are not stateful -- contents are cleared prior to each use.
+	/// They are only cached here to avoid extra memory allocations and deallocations and to ensure
+	/// that multiple instances of the solver can be run in parallel.
+	///
+	/// \{
+
+	/// Cache variable for constraint Jacobian matrix.
+	btMatrixXu m_scratchJ3;
+
+	/// Cache variable for constraint Jacobian times inverse mass matrix.
+	btMatrixXu m_scratchJInvM3;
+
+	/// Cache variable for offsets.
+	btAlignedObjectArray<int> m_scratchOfs;
+
+	/// \}
+
+	/// Constructs MLCP terms, which are \c m_A, \c m_b, \c m_lo, and \c m_hi.
+	virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
+
+	/// Constructs MLCP terms for constraints of two rigid bodies
+	void createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal);
+
+	/// Constructs MLCP terms for constraints of two multi-bodies or one rigid body and one multibody
+	void createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal);
+
+	/// Solves MLCP and returns the success
+	virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
+
+	// Documentation inherited
+	btScalar solveGroupCacheFriendlySetup(
+		btCollisionObject** bodies,
+		int numBodies,
+		btPersistentManifold** manifoldPtr,
+		int numManifolds,
+		btTypedConstraint** constraints,
+		int numConstraints,
+		const btContactSolverInfo& infoGlobal,
+		btIDebugDraw* debugDrawer) BT_OVERRIDE;
+
+	// Documentation inherited
+	btScalar solveGroupCacheFriendlyIterations(
+		btCollisionObject** bodies,
+		int numBodies,
+		btPersistentManifold** manifoldPtr,
+		int numManifolds,
+		btTypedConstraint** constraints,
+		int numConstraints,
+		const btContactSolverInfo& infoGlobal,
+		btIDebugDraw* debugDrawer) ;
+
+public:
+	BT_DECLARE_ALIGNED_ALLOCATOR()
+
+	/// Constructor
+	///
+	/// \param[in] solver MLCP solver. Assumed it's not null.
+	/// \param[in] maxLCPSize Maximum size of LCP to solve using MLCP solver. If the MLCP size exceeds this number, sequaltial impulse method will be used.
+	explicit btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver);
+
+	/// Destructor
+	virtual ~btMultiBodyMLCPConstraintSolver();
+
+	/// Sets MLCP solver. Assumed it's not null.
+	void setMLCPSolver(btMLCPSolverInterface* solver);
+
+	/// Returns the number of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP
+	/// solver fails.
+	int getNumFallbacks() const;
+
+	/// Sets the number of fallbacks. This function may be used to reset the number to zero.
+	void setNumFallbacks(int num);
+
+	/// Returns the constraint solver type.
+	virtual btConstraintSolverType getSolverType() const;
+};
+
+#endif  // BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
index 12b21f74603..f51e69deb1c 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
@@ -21,29 +21,29 @@ subject to the following restrictions:
 #include "LinearMath/btIDebugDraw.h"
 
 #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
-	#define BTMBP2PCONSTRAINT_DIM 3
+#define BTMBP2PCONSTRAINT_DIM 3
 #else
-	#define BTMBP2PCONSTRAINT_DIM 6
+#define BTMBP2PCONSTRAINT_DIM 6
 #endif
 
 btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
-	:btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false),
-	m_rigidBodyA(0),
-	m_rigidBodyB(bodyB),
-	m_pivotInA(pivotInA),
-	m_pivotInB(pivotInB)
+	: btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(bodyB),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB)
 {
-    m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
+	m_data.resize(BTMBP2PCONSTRAINT_DIM);  //at least store the applied impulses
 }
 
 btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
-	:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false),
-	m_rigidBodyA(0),
-	m_rigidBodyB(0),
-	m_pivotInA(pivotInA),
-	m_pivotInB(pivotInB)
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(0),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB)
 {
-    m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
+	m_data.resize(BTMBP2PCONSTRAINT_DIM);  //at least store the applied impulses
 }
 
 void btMultiBodyPoint2Point::finalizeMultiDof()
@@ -56,7 +56,6 @@ btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
 {
 }
 
-
 int btMultiBodyPoint2Point::getIslandIdA() const
 {
 	if (m_rigidBodyA)
@@ -64,13 +63,16 @@ int btMultiBodyPoint2Point::getIslandIdA() const
 
 	if (m_bodyA)
 	{
-		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-		for (int i=0;i<m_bodyA->getNumLinks();i++)
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
 		{
-			if (m_bodyA->getLink(i).m_collider)
-				return m_bodyA->getLink(i).m_collider->getIslandTag();
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
 		}
 	}
 	return -1;
@@ -82,62 +84,58 @@ int btMultiBodyPoint2Point::getIslandIdB() const
 		return m_rigidBodyB->getIslandTag();
 	if (m_bodyB)
 	{
-		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
-		if (col)
-			return col->getIslandTag();
-
-		for (int i=0;i<m_bodyB->getNumLinks();i++)
+		if (m_linkB < 0)
 		{
-			col = m_bodyB->getLink(i).m_collider;
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
 			if (col)
 				return col->getIslandTag();
 		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
 	}
 	return -1;
 }
 
-
-
 void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal)
+												  btMultiBodyJacobianData& data,
+												  const btContactSolverInfo& infoGlobal)
 {
-
-//	int i=1;
-int numDim = BTMBP2PCONSTRAINT_DIM;
-	for (int i=0;i<numDim;i++)
+	//	int i=1;
+	int numDim = BTMBP2PCONSTRAINT_DIM;
+	for (int i = 0; i < numDim; i++)
 	{
-
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
-        //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
-	constraintRow.m_orgConstraint = this;
-	constraintRow.m_orgDofIndex = i;
-        constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
-        constraintRow.m_contactNormal1.setValue(0,0,0);
-        constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
-        constraintRow.m_contactNormal2.setValue(0,0,0);
-        constraintRow.m_angularComponentA.setValue(0,0,0);
-        constraintRow.m_angularComponentB.setValue(0,0,0);
+		//memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = i;
+		constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal1.setValue(0, 0, 0);
+		constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal2.setValue(0, 0, 0);
+		constraintRow.m_angularComponentA.setValue(0, 0, 0);
+		constraintRow.m_angularComponentB.setValue(0, 0, 0);
 
 		constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
 		constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
 
-		btVector3 contactNormalOnB(0,0,0);
+		btVector3 contactNormalOnB(0, 0, 0);
 #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
 		contactNormalOnB[i] = -1;
 #else
-		contactNormalOnB[i%3] = -1;
+		contactNormalOnB[i % 3] = -1;
 #endif
 
-
-		 // Convert local points back to world
+		// Convert local points back to world
 		btVector3 pivotAworld = m_pivotInA;
 		if (m_rigidBodyA)
 		{
-
 			constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
-			pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
-		} else
+			pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+		}
+		else
 		{
 			if (m_bodyA)
 				pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
@@ -146,44 +144,41 @@ int numDim = BTMBP2PCONSTRAINT_DIM;
 		if (m_rigidBodyB)
 		{
 			constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
-			pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
-		} else
+			pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+		}
+		else
 		{
 			if (m_bodyB)
 				pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
-
 		}
 
-		btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0;
+		btScalar posError = i < 3 ? (pivotAworld - pivotBworld).dot(contactNormalOnB) : 0;
 
 #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
 
-
-		fillMultiBodyConstraint(constraintRow, data, 0, 0,
-															contactNormalOnB, pivotAworld, pivotBworld,						//sucks but let it be this way "for the time being"
-															posError,
-															infoGlobal,
-															-m_maxAppliedImpulse, m_maxAppliedImpulse
-															);
-    //@todo: support the case of btMultiBody versus btRigidBody,
-    //see btPoint2PointConstraint::getInfo2NonVirtual
+		fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0, 0, 0),
+								contactNormalOnB, pivotAworld, pivotBworld,  //sucks but let it be this way "for the time being"
+								posError,
+								infoGlobal,
+								-m_maxAppliedImpulse, m_maxAppliedImpulse);
+		//@todo: support the case of btMultiBody versus btRigidBody,
+		//see btPoint2PointConstraint::getInfo2NonVirtual
 #else
 		const btVector3 dummy(0, 0, 0);
 
 		btAssert(m_bodyA->isMultiDof());
 
 		btScalar* jac1 = jacobianA(i);
-		const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy;
-		const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy;
+		const btVector3& normalAng = i >= 3 ? contactNormalOnB : dummy;
+		const btVector3& normalLin = i < 3 ? contactNormalOnB : dummy;
 
 		m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
 
 		fillMultiBodyConstraint(constraintRow, data, jac1, 0,
-													dummy, dummy, dummy,						//sucks but let it be this way "for the time being"
-													posError,
-													infoGlobal,
-													-m_maxAppliedImpulse, m_maxAppliedImpulse
-													);
+								dummy, dummy, dummy,  //sucks but let it be this way "for the time being"
+								posError,
+								infoGlobal,
+								-m_maxAppliedImpulse, m_maxAppliedImpulse);
 #endif
 	}
 }
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h
index b2e219ac159..ef03a557ec5 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h
@@ -22,20 +22,20 @@ subject to the following restrictions:
 
 //#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
 
-class btMultiBodyPoint2Point : public btMultiBodyConstraint
+ATTRIBUTE_ALIGNED16(class)
+btMultiBodyPoint2Point : public btMultiBodyConstraint
 {
 protected:
-
-	btRigidBody*	m_rigidBodyA;
-	btRigidBody*	m_rigidBodyB;
-	btVector3		m_pivotInA;
-	btVector3		m_pivotInB;
-
+	btRigidBody* m_rigidBodyA;
+	btRigidBody* m_rigidBodyB;
+	btVector3 m_pivotInA;
+	btVector3 m_pivotInB;
 
 public:
+	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB);
-	btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB);
+	btMultiBodyPoint2Point(btMultiBody * body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB);
+	btMultiBodyPoint2Point(btMultiBody * bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB);
 
 	virtual ~btMultiBodyPoint2Point();
 
@@ -44,22 +44,21 @@ public:
 	virtual int getIslandIdA() const;
 	virtual int getIslandIdB() const;
 
-	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
-		btMultiBodyJacobianData& data,
-		const btContactSolverInfo& infoGlobal);
+	virtual void createConstraintRows(btMultiBodyConstraintArray & constraintRows,
+									  btMultiBodyJacobianData & data,
+									  const btContactSolverInfo& infoGlobal);
 
 	const btVector3& getPivotInB() const
 	{
 		return m_pivotInB;
 	}
 
-	void setPivotInB(const btVector3& pivotInB)
+	virtual void setPivotInB(const btVector3& pivotInB)
 	{
 		m_pivotInB = pivotInB;
 	}
 
-	virtual void debugDraw(class btIDebugDraw* drawer);
-
+	virtual void debugDraw(class btIDebugDraw * drawer);
 };
 
-#endif //BT_MULTIBODY_POINT2POINT_H
+#endif  //BT_MULTIBODY_POINT2POINT_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
new file mode 100644
index 00000000000..48ec1d5af2b
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
@@ -0,0 +1,234 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodySliderConstraint.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "LinearMath/btIDebugDraw.h"
+
+#define BTMBSLIDERCONSTRAINT_DIM 5
+#define EPSILON 0.000001
+
+btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
+	: btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(bodyB),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB),
+	  m_jointAxis(jointAxis)
+{
+	m_data.resize(BTMBSLIDERCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
+	: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER),
+	  m_rigidBodyA(0),
+	  m_rigidBodyB(0),
+	  m_pivotInA(pivotInA),
+	  m_pivotInB(pivotInB),
+	  m_frameInA(frameInA),
+	  m_frameInB(frameInB),
+	  m_jointAxis(jointAxis)
+{
+	m_data.resize(BTMBSLIDERCONSTRAINT_DIM);  //at least store the applied impulses
+}
+
+void btMultiBodySliderConstraint::finalizeMultiDof()
+{
+	//not implemented yet
+	btAssert(0);
+}
+
+btMultiBodySliderConstraint::~btMultiBodySliderConstraint()
+{
+}
+
+int btMultiBodySliderConstraint::getIslandIdA() const
+{
+	if (m_rigidBodyA)
+		return m_rigidBodyA->getIslandTag();
+
+	if (m_bodyA)
+	{
+		if (m_linkA < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyA->getLink(m_linkA).m_collider)
+				return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodySliderConstraint::getIslandIdB() const
+{
+	if (m_rigidBodyB)
+		return m_rigidBodyB->getIslandTag();
+	if (m_bodyB)
+	{
+		if (m_linkB < 0)
+		{
+			btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+			if (col)
+				return col->getIslandTag();
+		}
+		else
+		{
+			if (m_bodyB->getLink(m_linkB).m_collider)
+				return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
+{
+	// Convert local points back to world
+	btVector3 pivotAworld = m_pivotInA;
+	btMatrix3x3 frameAworld = m_frameInA;
+	btVector3 jointAxis = m_jointAxis;
+	if (m_rigidBodyA)
+	{
+		pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+		frameAworld = m_frameInA.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
+		jointAxis = quatRotate(m_rigidBodyA->getOrientation(), m_jointAxis);
+	}
+	else if (m_bodyA)
+	{
+		pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+		frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA);
+		jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis);
+	}
+	btVector3 pivotBworld = m_pivotInB;
+	btMatrix3x3 frameBworld = m_frameInB;
+	if (m_rigidBodyB)
+	{
+		pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+		frameBworld = m_frameInB.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
+	}
+	else if (m_bodyB)
+	{
+		pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+		frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);
+	}
+
+	btVector3 constraintAxis[2];
+	for (int i = 0; i < 3; ++i)
+	{
+		constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
+		if (constraintAxis[0].safeNorm() > EPSILON)
+		{
+			constraintAxis[0] = constraintAxis[0].normalized();
+			constraintAxis[1] = jointAxis.cross(constraintAxis[0]);
+			constraintAxis[1] = constraintAxis[1].normalized();
+			break;
+		}
+	}
+
+	btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
+	btVector3 angleDiff;
+	btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
+
+	int numDim = BTMBSLIDERCONSTRAINT_DIM;
+	for (int i = 0; i < numDim; i++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = i;
+		constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal1.setValue(0, 0, 0);
+		constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
+		constraintRow.m_contactNormal2.setValue(0, 0, 0);
+		constraintRow.m_angularComponentA.setValue(0, 0, 0);
+		constraintRow.m_angularComponentB.setValue(0, 0, 0);
+
+		constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
+		constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
+
+		if (m_rigidBodyA)
+		{
+			constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
+		}
+		if (m_rigidBodyB)
+		{
+			constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
+		}
+
+		btVector3 constraintNormalLin(0, 0, 0);
+		btVector3 constraintNormalAng(0, 0, 0);
+		btScalar posError = 0.0;
+		if (i < 2)
+		{
+			constraintNormalLin = constraintAxis[i];
+			posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
+			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+									constraintNormalLin, pivotAworld, pivotBworld,
+									posError,
+									infoGlobal,
+									-m_maxAppliedImpulse, m_maxAppliedImpulse);
+		}
+		else
+		{  //i>=2
+			constraintNormalAng = frameAworld.getColumn(i % 3);
+			posError = angleDiff[i % 3];
+			fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+									constraintNormalLin, pivotAworld, pivotBworld,
+									posError,
+									infoGlobal,
+									-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
+		}
+	}
+}
+
+void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer)
+{
+	btTransform tr;
+	tr.setIdentity();
+
+	if (m_rigidBodyA)
+	{
+		btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyA)
+	{
+		btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+		tr.setOrigin(pivotAworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_rigidBodyB)
+	{
+		// that ideally should draw the same frame
+		btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyB)
+	{
+		btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+		tr.setOrigin(pivotBworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+}
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h
new file mode 100644
index 00000000000..b192b6f8f37
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h
@@ -0,0 +1,102 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_SLIDER_CONSTRAINT_H
+#define BT_MULTIBODY_SLIDER_CONSTRAINT_H
+
+#include "btMultiBodyConstraint.h"
+
+class btMultiBodySliderConstraint : public btMultiBodyConstraint
+{
+protected:
+	btRigidBody* m_rigidBodyA;
+	btRigidBody* m_rigidBodyB;
+	btVector3 m_pivotInA;
+	btVector3 m_pivotInB;
+	btMatrix3x3 m_frameInA;
+	btMatrix3x3 m_frameInB;
+	btVector3 m_jointAxis;
+
+public:
+	btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis);
+	btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis);
+
+	virtual ~btMultiBodySliderConstraint();
+
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	const btVector3& getPivotInA() const
+	{
+		return m_pivotInA;
+	}
+
+	void setPivotInA(const btVector3& pivotInA)
+	{
+		m_pivotInA = pivotInA;
+	}
+
+	const btVector3& getPivotInB() const
+	{
+		return m_pivotInB;
+	}
+
+	virtual void setPivotInB(const btVector3& pivotInB)
+	{
+		m_pivotInB = pivotInB;
+	}
+
+	const btMatrix3x3& getFrameInA() const
+	{
+		return m_frameInA;
+	}
+
+	void setFrameInA(const btMatrix3x3& frameInA)
+	{
+		m_frameInA = frameInA;
+	}
+
+	const btMatrix3x3& getFrameInB() const
+	{
+		return m_frameInB;
+	}
+
+	virtual void setFrameInB(const btMatrix3x3& frameInB)
+	{
+		m_frameInB = frameInB;
+	}
+
+	const btVector3& getJointAxis() const
+	{
+		return m_jointAxis;
+	}
+
+	void setJointAxis(const btVector3& jointAxis)
+	{
+		m_jointAxis = jointAxis;
+	}
+
+	virtual void debugDraw(class btIDebugDraw* drawer);
+};
+
+#endif  //BT_MULTIBODY_SLIDER_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h
index 6fa1550e9e6..deed3e2a123 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h
@@ -25,66 +25,66 @@ class btMultiBodyConstraint;
 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
 
 ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
-ATTRIBUTE_ALIGNED16 (struct)	btMultiBodySolverConstraint
+ATTRIBUTE_ALIGNED16(struct)
+btMultiBodySolverConstraint
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_orgConstraint(0), m_orgDofIndex(-1)
-	{}
-
-	int				m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
-	int				m_jacAindex;
-	int				m_deltaVelBindex;
-	int				m_jacBindex;
-
-	btVector3		m_relpos1CrossNormal;
-	btVector3		m_contactNormal1;	
-	btVector3		m_relpos2CrossNormal;
-	btVector3		m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always
-	
-
-	btVector3		m_angularComponentA;
-	btVector3		m_angularComponentB;
-	
-	mutable btSimdScalar	m_appliedPushImpulse;
-	mutable btSimdScalar	m_appliedImpulse;
-
-	btScalar	m_friction;
-	btScalar	m_jacDiagABInv;
-	btScalar		m_rhs;
-	btScalar		m_cfm;
-	
-    btScalar		m_lowerLimit;
-	btScalar		m_upperLimit;
-	btScalar		m_rhsPenetration;
-    union
+	btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1), m_orgConstraint(0), m_orgDofIndex(-1)
 	{
-		void*		m_originalContactPoint;
-		btScalar	m_unusedPadding4;
+	}
+
+	int m_deltaVelAindex;  //more generic version of m_relpos1CrossNormal/m_contactNormal1
+	int m_jacAindex;
+	int m_deltaVelBindex;
+	int m_jacBindex;
+
+	btVector3 m_relpos1CrossNormal;
+	btVector3 m_contactNormal1;
+	btVector3 m_relpos2CrossNormal;
+	btVector3 m_contactNormal2;  //usually m_contactNormal2 == -m_contactNormal1, but not always
+
+	btVector3 m_angularComponentA;
+	btVector3 m_angularComponentB;
+
+	mutable btSimdScalar m_appliedPushImpulse;
+	mutable btSimdScalar m_appliedImpulse;
+
+	btScalar m_friction;
+	btScalar m_jacDiagABInv;
+	btScalar m_rhs;
+	btScalar m_cfm;
+
+	btScalar m_lowerLimit;
+	btScalar m_upperLimit;
+	btScalar m_rhsPenetration;
+	union {
+		void* m_originalContactPoint;
+		btScalar m_unusedPadding4;
 	};
 
-	int	m_overrideNumSolverIterations;
-    int			m_frictionIndex;
+	int m_overrideNumSolverIterations;
+	int m_frictionIndex;
 
 	int m_solverBodyIdA;
 	btMultiBody* m_multiBodyA;
-	int			m_linkA;
-	
+	int m_linkA;
+
 	int m_solverBodyIdB;
 	btMultiBody* m_multiBodyB;
-	int			m_linkB;
+	int m_linkB;
 
 	//for writing back applied impulses
-	btMultiBodyConstraint*	m_orgConstraint;
+	btMultiBodyConstraint* m_orgConstraint;
 	int m_orgDofIndex;
 
-	enum		btSolverConstraintType
+	enum btSolverConstraintType
 	{
 		BT_SOLVER_CONTACT_1D = 0,
 		BT_SOLVER_FRICTION_1D
 	};
 };
 
-typedef btAlignedObjectArray<btMultiBodySolverConstraint>	btMultiBodyConstraintArray;
+typedef btAlignedObjectArray<btMultiBodySolverConstraint> btMultiBodyConstraintArray;
 
-#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H
+#endif  //BT_MULTIBODY_SOLVER_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp
new file mode 100644
index 00000000000..25ddd539bf9
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp
@@ -0,0 +1,172 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodySphericalJointMotor.h"
+#include "btMultiBody.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "LinearMath/btTransformUtil.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+
+btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse)
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR),
+	m_desiredVelocity(0, 0, 0),
+	m_desiredPosition(0,0,0,1),
+	m_kd(1.),
+	m_kp(0.2),
+	m_erp(1),
+	m_rhsClamp(SIMD_INFINITY)
+{
+
+	m_maxAppliedImpulse = maxMotorImpulse;
+}
+
+
+void btMultiBodySphericalJointMotor::finalizeMultiDof()
+{
+	allocateJacobiansMultiDof();
+	// note: we rely on the fact that data.m_jacobians are
+	// always initialized to zero by the Constraint ctor
+	int linkDoF = 0;
+	unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
+
+	// row 0: the lower bound
+	// row 0: the lower bound
+	jacobianA(0)[offset] = 1;
+
+	m_numDofsFinalized = m_jacSizeBoth;
+}
+
+
+btMultiBodySphericalJointMotor::~btMultiBodySphericalJointMotor()
+{
+}
+
+int btMultiBodySphericalJointMotor::getIslandIdA() const
+{
+	if (this->m_linkA < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyA->getLink(m_linkA).m_collider)
+		{
+			return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodySphericalJointMotor::getIslandIdB() const
+{
+	if (m_linkB < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyB->getLink(m_linkB).m_collider)
+		{
+			return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodySphericalJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+												 btMultiBodyJacobianData& data,
+												 const btContactSolverInfo& infoGlobal)
+{
+	// only positions need to be updated -- data.m_jacobians and force
+	// directions were set in the ctor and never change.
+
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+		finalizeMultiDof();
+	}
+
+	//don't crash
+	if (m_numDofsFinalized != m_jacSizeBoth)
+		return;
+	
+
+	if (m_maxAppliedImpulse == 0.f)
+		return;
+
+	const btScalar posError = 0;
+	const btVector3 dummy(0, 0, 0);
+
+	
+	btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) };
+	
+	btQuaternion desiredQuat = m_desiredPosition;
+	btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0],
+		m_bodyA->getJointPosMultiDof(m_linkA)[1],
+		m_bodyA->getJointPosMultiDof(m_linkA)[2],
+		m_bodyA->getJointPosMultiDof(m_linkA)[3]);
+
+btQuaternion relRot = currentQuat.inverse() * desiredQuat;
+	btVector3 angleDiff;
+	btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff);
+
+
+
+	for (int row = 0; row < getNumRows(); row++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+
+		int dof = row;
+		
+		btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+		btScalar desiredVelocity = this->m_desiredVelocity[row];
+		
+		btScalar velocityError = desiredVelocity - currentVelocity;
+
+		btMatrix3x3 frameAworld;
+		frameAworld.setIdentity();
+		frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
+		btScalar posError = 0;
+		{
+			btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical);
+			switch (m_bodyA->getLink(m_linkA).m_jointType)
+			{
+				case btMultibodyLink::eSpherical:
+				{
+					btVector3 constraintNormalAng = frameAworld.getColumn(row % 3);
+					posError = m_kp*angleDiff[row % 3];
+					fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+						btVector3(0,0,0), dummy, dummy,
+						posError,
+						infoGlobal,
+						-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
+					constraintRow.m_orgConstraint = this;
+					constraintRow.m_orgDofIndex = row;
+					break;
+				}
+				default:
+				{
+					btAssert(0);
+				}
+			};
+		}
+	}
+}
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h
new file mode 100644
index 00000000000..621beab5a42
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h
@@ -0,0 +1,77 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H
+#define BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H
+
+#include "btMultiBodyConstraint.h"
+struct btSolverInfo;
+
+class btMultiBodySphericalJointMotor : public btMultiBodyConstraint
+{
+protected:
+	btVector3 m_desiredVelocity;
+	btQuaternion m_desiredPosition;
+	btScalar m_kd;
+	btScalar m_kp;
+	btScalar m_erp;
+	btScalar m_rhsClamp;  //maximum error
+
+public:
+	btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse);
+	
+	virtual ~btMultiBodySphericalJointMotor();
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.f)
+	{
+		m_desiredVelocity = velTarget;
+		m_kd = kd;
+	}
+
+	virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp = 1.f)
+	{
+		m_desiredPosition = posTarget;
+		m_kp = kp;
+	}
+
+	virtual void setErp(btScalar erp)
+	{
+		m_erp = erp;
+	}
+	virtual btScalar getErp() const
+	{
+		return m_erp;
+	}
+	virtual void setRhsClamp(btScalar rhsClamp)
+	{
+		m_rhsClamp = rhsClamp;
+	}
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
+};
+
+#endif  //BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp
index 986f2148701..98ecdc07947 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp
@@ -108,18 +108,16 @@ rows/columns and manipulate C.
 
 */
 
-
 #include "btDantzigLCP.h"
 
-#include <string.h>//memcpy
+#include <string.h>  //memcpy
 
 bool s_error = false;
 
 //***************************************************************************
 // code generation parameters
 
-
-#define btLCP_FAST		// use fast btLCP object
+#define btLCP_FAST  // use fast btLCP object
 
 // option 1 : matrix row pointers (less data copying)
 #define BTROWPTRS
@@ -133,8 +131,6 @@ bool s_error = false;
 
 #define BTNUB_OPTIMIZATIONS
 
-
-
 /* solve L*X=B, with B containing 1 right hand sides.
  * L is an n*n lower triangular matrix with ones on the diagonal.
  * L is stored by rows and its leading dimension is lskip.
@@ -145,66 +141,69 @@ bool s_error = false;
  * if this is in the factorizer source file, n must be a multiple of 2.
  */
 
-static void btSolveL1_1 (const btScalar *L, btScalar *B, int n, int lskip1)
-{  
-  /* declare variables - Z matrix, p and q vectors, etc */
-  btScalar Z11,m11,Z21,m21,p1,q1,p2,*ex;
-  const btScalar *ell;
-  int i,j;
-  /* compute all 2 x 1 blocks of X */
-  for (i=0; i < n; i+=2) {
-    /* compute all 2 x 1 block of X, from rows i..i+2-1 */
-    /* set the Z matrix to 0 */
-    Z11=0;
-    Z21=0;
-    ell = L + i*lskip1;
-    ex = B;
-    /* the inner loop that computes outer products and adds them to Z */
-    for (j=i-2; j >= 0; j -= 2) {
-      /* compute outer product and add it to the Z matrix */
-      p1=ell[0];
-      q1=ex[0];
-      m11 = p1 * q1;
-      p2=ell[lskip1];
-      m21 = p2 * q1;
-      Z11 += m11;
-      Z21 += m21;
-      /* compute outer product and add it to the Z matrix */
-      p1=ell[1];
-      q1=ex[1];
-      m11 = p1 * q1;
-      p2=ell[1+lskip1];
-      m21 = p2 * q1;
-      /* advance pointers */
-      ell += 2;
-      ex += 2;
-      Z11 += m11;
-      Z21 += m21;
-      /* end of inner loop */
-    }
-    /* compute left-over iterations */
-    j += 2;
-    for (; j > 0; j--) {
-      /* compute outer product and add it to the Z matrix */
-      p1=ell[0];
-      q1=ex[0];
-      m11 = p1 * q1;
-      p2=ell[lskip1];
-      m21 = p2 * q1;
-      /* advance pointers */
-      ell += 1;
-      ex += 1;
-      Z11 += m11;
-      Z21 += m21;
-    }
-    /* finish computing the X(i) block */
-    Z11 = ex[0] - Z11;
-    ex[0] = Z11;
-    p1 = ell[lskip1];
-    Z21 = ex[1] - Z21 - p1*Z11;
-    ex[1] = Z21;
-    /* end of outer loop */
-  }
+static void btSolveL1_1(const btScalar *L, btScalar *B, int n, int lskip1)
+{
+	/* declare variables - Z matrix, p and q vectors, etc */
+	btScalar Z11, m11, Z21, m21, p1, q1, p2, *ex;
+	const btScalar *ell;
+	int i, j;
+	/* compute all 2 x 1 blocks of X */
+	for (i = 0; i < n; i += 2)
+	{
+		/* compute all 2 x 1 block of X, from rows i..i+2-1 */
+		/* set the Z matrix to 0 */
+		Z11 = 0;
+		Z21 = 0;
+		ell = L + i * lskip1;
+		ex = B;
+		/* the inner loop that computes outer products and adds them to Z */
+		for (j = i - 2; j >= 0; j -= 2)
+		{
+			/* compute outer product and add it to the Z matrix */
+			p1 = ell[0];
+			q1 = ex[0];
+			m11 = p1 * q1;
+			p2 = ell[lskip1];
+			m21 = p2 * q1;
+			Z11 += m11;
+			Z21 += m21;
+			/* compute outer product and add it to the Z matrix */
+			p1 = ell[1];
+			q1 = ex[1];
+			m11 = p1 * q1;
+			p2 = ell[1 + lskip1];
+			m21 = p2 * q1;
+			/* advance pointers */
+			ell += 2;
+			ex += 2;
+			Z11 += m11;
+			Z21 += m21;
+			/* end of inner loop */
+		}
+		/* compute left-over iterations */
+		j += 2;
+		for (; j > 0; j--)
+		{
+			/* compute outer product and add it to the Z matrix */
+			p1 = ell[0];
+			q1 = ex[0];
+			m11 = p1 * q1;
+			p2 = ell[lskip1];
+			m21 = p2 * q1;
+			/* advance pointers */
+			ell += 1;
+			ex += 1;
+			Z11 += m11;
+			Z21 += m21;
+		}
+		/* finish computing the X(i) block */
+		Z11 = ex[0] - Z11;
+		ex[0] = Z11;
+		p1 = ell[lskip1];
+		Z21 = ex[1] - Z21 - p1 * Z11;
+		ex[1] = Z21;
+		/* end of outer loop */
+	}
 }
 
 /* solve L*X=B, with B containing 2 right hand sides.
@@ -217,300 +216,308 @@ static void btSolveL1_1 (const btScalar *L, btScalar *B, int n, int lskip1)
  * if this is in the factorizer source file, n must be a multiple of 2.
  */
 
-static void btSolveL1_2 (const btScalar *L, btScalar *B, int n, int lskip1)
-{  
-  /* declare variables - Z matrix, p and q vectors, etc */
-  btScalar Z11,m11,Z12,m12,Z21,m21,Z22,m22,p1,q1,p2,q2,*ex;
-  const btScalar *ell;
-  int i,j;
-  /* compute all 2 x 2 blocks of X */
-  for (i=0; i < n; i+=2) {
-    /* compute all 2 x 2 block of X, from rows i..i+2-1 */
-    /* set the Z matrix to 0 */
-    Z11=0;
-    Z12=0;
-    Z21=0;
-    Z22=0;
-    ell = L + i*lskip1;
-    ex = B;
-    /* the inner loop that computes outer products and adds them to Z */
-    for (j=i-2; j >= 0; j -= 2) {
-      /* compute outer product and add it to the Z matrix */
-      p1=ell[0];
-      q1=ex[0];
-      m11 = p1 * q1;
-      q2=ex[lskip1];
-      m12 = p1 * q2;
-      p2=ell[lskip1];
-      m21 = p2 * q1;
-      m22 = p2 * q2;
-      Z11 += m11;
-      Z12 += m12;
-      Z21 += m21;
-      Z22 += m22;
-      /* compute outer product and add it to the Z matrix */
-      p1=ell[1];
-      q1=ex[1];
-      m11 = p1 * q1;
-      q2=ex[1+lskip1];
-      m12 = p1 * q2;
-      p2=ell[1+lskip1];
-      m21 = p2 * q1;
-      m22 = p2 * q2;
-      /* advance pointers */
-      ell += 2;
-      ex += 2;
-      Z11 += m11;
-      Z12 += m12;
-      Z21 += m21;
-      Z22 += m22;
-      /* end of inner loop */
-    }
-    /* compute left-over iterations */
-    j += 2;
-    for (; j > 0; j--) {
-      /* compute outer product and add it to the Z matrix */
-      p1=ell[0];
-      q1=ex[0];
-      m11 = p1 * q1;
-      q2=ex[lskip1];
-      m12 = p1 * q2;
-      p2=ell[lskip1];
-      m21 = p2 * q1;
-      m22 = p2 * q2;
-      /* advance pointers */
-      ell += 1;
-      ex += 1;
-      Z11 += m11;
-      Z12 += m12;
-      Z21 += m21;
-      Z22 += m22;
-    }
-    /* finish computing the X(i) block */
-    Z11 = ex[0] - Z11;
-    ex[0] = Z11;
-    Z12 = ex[lskip1] - Z12;
-    ex[lskip1] = Z12;
-    p1 = ell[lskip1];
-    Z21 = ex[1] - Z21 - p1*Z11;
-    ex[1] = Z21;
-    Z22 = ex[1+lskip1] - Z22 - p1*Z12;
-    ex[1+lskip1] = Z22;
-    /* end of outer loop */
-  }
+static void btSolveL1_2(const btScalar *L, btScalar *B, int n, int lskip1)
+{
+	/* declare variables - Z matrix, p and q vectors, etc */
+	btScalar Z11, m11, Z12, m12, Z21, m21, Z22, m22, p1, q1, p2, q2, *ex;
+	const btScalar *ell;
+	int i, j;
+	/* compute all 2 x 2 blocks of X */
+	for (i = 0; i < n; i += 2)
+	{
+		/* compute all 2 x 2 block of X, from rows i..i+2-1 */
+		/* set the Z matrix to 0 */
+		Z11 = 0;
+		Z12 = 0;
+		Z21 = 0;
+		Z22 = 0;
+		ell = L + i * lskip1;
+		ex = B;
+		/* the inner loop that computes outer products and adds them to Z */
+		for (j = i - 2; j >= 0; j -= 2)
+		{
+			/* compute outer product and add it to the Z matrix */
+			p1 = ell[0];
+			q1 = ex[0];
+			m11 = p1 * q1;
+			q2 = ex[lskip1];
+			m12 = p1 * q2;
+			p2 = ell[lskip1];
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			Z11 += m11;
+			Z12 += m12;
+			Z21 += m21;
+			Z22 += m22;
+			/* compute outer product and add it to the Z matrix */
+			p1 = ell[1];
+			q1 = ex[1];
+			m11 = p1 * q1;
+			q2 = ex[1 + lskip1];
+			m12 = p1 * q2;
+			p2 = ell[1 + lskip1];
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			/* advance pointers */
+			ell += 2;
+			ex += 2;
+			Z11 += m11;
+			Z12 += m12;
+			Z21 += m21;
+			Z22 += m22;
+			/* end of inner loop */
+		}
+		/* compute left-over iterations */
+		j += 2;
+		for (; j > 0; j--)
+		{
+			/* compute outer product and add it to the Z matrix */
+			p1 = ell[0];
+			q1 = ex[0];
+			m11 = p1 * q1;
+			q2 = ex[lskip1];
+			m12 = p1 * q2;
+			p2 = ell[lskip1];
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			/* advance pointers */
+			ell += 1;
+			ex += 1;
+			Z11 += m11;
+			Z12 += m12;
+			Z21 += m21;
+			Z22 += m22;
+		}
+		/* finish computing the X(i) block */
+		Z11 = ex[0] - Z11;
+		ex[0] = Z11;
+		Z12 = ex[lskip1] - Z12;
+		ex[lskip1] = Z12;
+		p1 = ell[lskip1];
+		Z21 = ex[1] - Z21 - p1 * Z11;
+		ex[1] = Z21;
+		Z22 = ex[1 + lskip1] - Z22 - p1 * Z12;
+		ex[1 + lskip1] = Z22;
+		/* end of outer loop */
+	}
 }
 
+void btFactorLDLT(btScalar *A, btScalar *d, int n, int nskip1)
+{
+	int i, j;
+	btScalar sum, *ell, *dee, dd, p1, p2, q1, q2, Z11, m11, Z21, m21, Z22, m22;
+	if (n < 1) return;
 
-void btFactorLDLT (btScalar *A, btScalar *d, int n, int nskip1)
-{  
-  int i,j;
-  btScalar sum,*ell,*dee,dd,p1,p2,q1,q2,Z11,m11,Z21,m21,Z22,m22;
-  if (n < 1) return;
-  
-  for (i=0; i<=n-2; i += 2) {
-    /* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */
-    btSolveL1_2 (A,A+i*nskip1,i,nskip1);
-    /* scale the elements in a 2 x i block at A(i,0), and also */
-    /* compute Z = the outer product matrix that we'll need. */
-    Z11 = 0;
-    Z21 = 0;
-    Z22 = 0;
-    ell = A+i*nskip1;
-    dee = d;
-    for (j=i-6; j >= 0; j -= 6) {
-      p1 = ell[0];
-      p2 = ell[nskip1];
-      dd = dee[0];
-      q1 = p1*dd;
-      q2 = p2*dd;
-      ell[0] = q1;
-      ell[nskip1] = q2;
-      m11 = p1*q1;
-      m21 = p2*q1;
-      m22 = p2*q2;
-      Z11 += m11;
-      Z21 += m21;
-      Z22 += m22;
-      p1 = ell[1];
-      p2 = ell[1+nskip1];
-      dd = dee[1];
-      q1 = p1*dd;
-      q2 = p2*dd;
-      ell[1] = q1;
-      ell[1+nskip1] = q2;
-      m11 = p1*q1;
-      m21 = p2*q1;
-      m22 = p2*q2;
-      Z11 += m11;
-      Z21 += m21;
-      Z22 += m22;
-      p1 = ell[2];
-      p2 = ell[2+nskip1];
-      dd = dee[2];
-      q1 = p1*dd;
-      q2 = p2*dd;
-      ell[2] = q1;
-      ell[2+nskip1] = q2;
-      m11 = p1*q1;
-      m21 = p2*q1;
-      m22 = p2*q2;
-      Z11 += m11;
-      Z21 += m21;
-      Z22 += m22;
-      p1 = ell[3];
-      p2 = ell[3+nskip1];
-      dd = dee[3];
-      q1 = p1*dd;
-      q2 = p2*dd;
-      ell[3] = q1;
-      ell[3+nskip1] = q2;
-      m11 = p1*q1;
-      m21 = p2*q1;
-      m22 = p2*q2;
-      Z11 += m11;
-      Z21 += m21;
-      Z22 += m22;
-      p1 = ell[4];
-      p2 = ell[4+nskip1];
-      dd = dee[4];
-      q1 = p1*dd;
-      q2 = p2*dd;
-      ell[4] = q1;
-      ell[4+nskip1] = q2;
-      m11 = p1*q1;
-      m21 = p2*q1;
-      m22 = p2*q2;
-      Z11 += m11;
-      Z21 += m21;
-      Z22 += m22;
-      p1 = ell[5];
-      p2 = ell[5+nskip1];
-      dd = dee[5];
-      q1 = p1*dd;
-      q2 = p2*dd;
-      ell[5] = q1;
-      ell[5+nskip1] = q2;
-      m11 = p1*q1;
-      m21 = p2*q1;
-      m22 = p2*q2;
-      Z11 += m11;
-      Z21 += m21;
-      Z22 += m22;
-      ell += 6;
-      dee += 6;
-    }
-    /* compute left-over iterations */
-    j += 6;
-    for (; j > 0; j--) {
-      p1 = ell[0];
-      p2 = ell[nskip1];
-      dd = dee[0];
-      q1 = p1*dd;
-      q2 = p2*dd;
-      ell[0] = q1;
-      ell[nskip1] = q2;
-      m11 = p1*q1;
-      m21 = p2*q1;
-      m22 = p2*q2;
-      Z11 += m11;
-      Z21 += m21;
-      Z22 += m22;
-      ell++;
-      dee++;
-    }
-    /* solve for diagonal 2 x 2 block at A(i,i) */
-    Z11 = ell[0] - Z11;
-    Z21 = ell[nskip1] - Z21;
-    Z22 = ell[1+nskip1] - Z22;
-    dee = d + i;
-    /* factorize 2 x 2 block Z,dee */
-    /* factorize row 1 */
-    dee[0] = btRecip(Z11);
-    /* factorize row 2 */
-    sum = 0;
-    q1 = Z21;
-    q2 = q1 * dee[0];
-    Z21 = q2;
-    sum += q1*q2;
-    dee[1] = btRecip(Z22 - sum);
-    /* done factorizing 2 x 2 block */
-    ell[nskip1] = Z21;
-  }
-  /* compute the (less than 2) rows at the bottom */
-  switch (n-i) {
-    case 0:
-    break;
-    
-    case 1:
-    btSolveL1_1 (A,A+i*nskip1,i,nskip1);
-    /* scale the elements in a 1 x i block at A(i,0), and also */
-    /* compute Z = the outer product matrix that we'll need. */
-    Z11 = 0;
-    ell = A+i*nskip1;
-    dee = d;
-    for (j=i-6; j >= 0; j -= 6) {
-      p1 = ell[0];
-      dd = dee[0];
-      q1 = p1*dd;
-      ell[0] = q1;
-      m11 = p1*q1;
-      Z11 += m11;
-      p1 = ell[1];
-      dd = dee[1];
-      q1 = p1*dd;
-      ell[1] = q1;
-      m11 = p1*q1;
-      Z11 += m11;
-      p1 = ell[2];
-      dd = dee[2];
-      q1 = p1*dd;
-      ell[2] = q1;
-      m11 = p1*q1;
-      Z11 += m11;
-      p1 = ell[3];
-      dd = dee[3];
-      q1 = p1*dd;
-      ell[3] = q1;
-      m11 = p1*q1;
-      Z11 += m11;
-      p1 = ell[4];
-      dd = dee[4];
-      q1 = p1*dd;
-      ell[4] = q1;
-      m11 = p1*q1;
-      Z11 += m11;
-      p1 = ell[5];
-      dd = dee[5];
-      q1 = p1*dd;
-      ell[5] = q1;
-      m11 = p1*q1;
-      Z11 += m11;
-      ell += 6;
-      dee += 6;
-    }
-    /* compute left-over iterations */
-    j += 6;
-    for (; j > 0; j--) {
-      p1 = ell[0];
-      dd = dee[0];
-      q1 = p1*dd;
-      ell[0] = q1;
-      m11 = p1*q1;
-      Z11 += m11;
-      ell++;
-      dee++;
-    }
-    /* solve for diagonal 1 x 1 block at A(i,i) */
-    Z11 = ell[0] - Z11;
-    dee = d + i;
-    /* factorize 1 x 1 block Z,dee */
-    /* factorize row 1 */
-    dee[0] = btRecip(Z11);
-    /* done factorizing 1 x 1 block */
-    break;
-    
-    //default: *((char*)0)=0;  /* this should never happen! */
-  }
+	for (i = 0; i <= n - 2; i += 2)
+	{
+		/* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */
+		btSolveL1_2(A, A + i * nskip1, i, nskip1);
+		/* scale the elements in a 2 x i block at A(i,0), and also */
+		/* compute Z = the outer product matrix that we'll need. */
+		Z11 = 0;
+		Z21 = 0;
+		Z22 = 0;
+		ell = A + i * nskip1;
+		dee = d;
+		for (j = i - 6; j >= 0; j -= 6)
+		{
+			p1 = ell[0];
+			p2 = ell[nskip1];
+			dd = dee[0];
+			q1 = p1 * dd;
+			q2 = p2 * dd;
+			ell[0] = q1;
+			ell[nskip1] = q2;
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			Z11 += m11;
+			Z21 += m21;
+			Z22 += m22;
+			p1 = ell[1];
+			p2 = ell[1 + nskip1];
+			dd = dee[1];
+			q1 = p1 * dd;
+			q2 = p2 * dd;
+			ell[1] = q1;
+			ell[1 + nskip1] = q2;
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			Z11 += m11;
+			Z21 += m21;
+			Z22 += m22;
+			p1 = ell[2];
+			p2 = ell[2 + nskip1];
+			dd = dee[2];
+			q1 = p1 * dd;
+			q2 = p2 * dd;
+			ell[2] = q1;
+			ell[2 + nskip1] = q2;
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			Z11 += m11;
+			Z21 += m21;
+			Z22 += m22;
+			p1 = ell[3];
+			p2 = ell[3 + nskip1];
+			dd = dee[3];
+			q1 = p1 * dd;
+			q2 = p2 * dd;
+			ell[3] = q1;
+			ell[3 + nskip1] = q2;
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			Z11 += m11;
+			Z21 += m21;
+			Z22 += m22;
+			p1 = ell[4];
+			p2 = ell[4 + nskip1];
+			dd = dee[4];
+			q1 = p1 * dd;
+			q2 = p2 * dd;
+			ell[4] = q1;
+			ell[4 + nskip1] = q2;
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			Z11 += m11;
+			Z21 += m21;
+			Z22 += m22;
+			p1 = ell[5];
+			p2 = ell[5 + nskip1];
+			dd = dee[5];
+			q1 = p1 * dd;
+			q2 = p2 * dd;
+			ell[5] = q1;
+			ell[5 + nskip1] = q2;
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			Z11 += m11;
+			Z21 += m21;
+			Z22 += m22;
+			ell += 6;
+			dee += 6;
+		}
+		/* compute left-over iterations */
+		j += 6;
+		for (; j > 0; j--)
+		{
+			p1 = ell[0];
+			p2 = ell[nskip1];
+			dd = dee[0];
+			q1 = p1 * dd;
+			q2 = p2 * dd;
+			ell[0] = q1;
+			ell[nskip1] = q2;
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m22 = p2 * q2;
+			Z11 += m11;
+			Z21 += m21;
+			Z22 += m22;
+			ell++;
+			dee++;
+		}
+		/* solve for diagonal 2 x 2 block at A(i,i) */
+		Z11 = ell[0] - Z11;
+		Z21 = ell[nskip1] - Z21;
+		Z22 = ell[1 + nskip1] - Z22;
+		dee = d + i;
+		/* factorize 2 x 2 block Z,dee */
+		/* factorize row 1 */
+		dee[0] = btRecip(Z11);
+		/* factorize row 2 */
+		sum = 0;
+		q1 = Z21;
+		q2 = q1 * dee[0];
+		Z21 = q2;
+		sum += q1 * q2;
+		dee[1] = btRecip(Z22 - sum);
+		/* done factorizing 2 x 2 block */
+		ell[nskip1] = Z21;
+	}
+	/* compute the (less than 2) rows at the bottom */
+	switch (n - i)
+	{
+		case 0:
+			break;
+
+		case 1:
+			btSolveL1_1(A, A + i * nskip1, i, nskip1);
+			/* scale the elements in a 1 x i block at A(i,0), and also */
+			/* compute Z = the outer product matrix that we'll need. */
+			Z11 = 0;
+			ell = A + i * nskip1;
+			dee = d;
+			for (j = i - 6; j >= 0; j -= 6)
+			{
+				p1 = ell[0];
+				dd = dee[0];
+				q1 = p1 * dd;
+				ell[0] = q1;
+				m11 = p1 * q1;
+				Z11 += m11;
+				p1 = ell[1];
+				dd = dee[1];
+				q1 = p1 * dd;
+				ell[1] = q1;
+				m11 = p1 * q1;
+				Z11 += m11;
+				p1 = ell[2];
+				dd = dee[2];
+				q1 = p1 * dd;
+				ell[2] = q1;
+				m11 = p1 * q1;
+				Z11 += m11;
+				p1 = ell[3];
+				dd = dee[3];
+				q1 = p1 * dd;
+				ell[3] = q1;
+				m11 = p1 * q1;
+				Z11 += m11;
+				p1 = ell[4];
+				dd = dee[4];
+				q1 = p1 * dd;
+				ell[4] = q1;
+				m11 = p1 * q1;
+				Z11 += m11;
+				p1 = ell[5];
+				dd = dee[5];
+				q1 = p1 * dd;
+				ell[5] = q1;
+				m11 = p1 * q1;
+				Z11 += m11;
+				ell += 6;
+				dee += 6;
+			}
+			/* compute left-over iterations */
+			j += 6;
+			for (; j > 0; j--)
+			{
+				p1 = ell[0];
+				dd = dee[0];
+				q1 = p1 * dd;
+				ell[0] = q1;
+				m11 = p1 * q1;
+				Z11 += m11;
+				ell++;
+				dee++;
+			}
+			/* solve for diagonal 1 x 1 block at A(i,i) */
+			Z11 = ell[0] - Z11;
+			dee = d + i;
+			/* factorize 1 x 1 block Z,dee */
+			/* factorize row 1 */
+			dee[0] = btRecip(Z11);
+			/* done factorizing 1 x 1 block */
+			break;
+
+			//default: *((char*)0)=0;  /* this should never happen! */
+	}
 }
 
 /* solve L*X=B, with B containing 1 right hand sides.
@@ -523,289 +530,295 @@ void btFactorLDLT (btScalar *A, btScalar *d, int n, int nskip1)
  * if this is in the factorizer source file, n must be a multiple of 4.
  */
 
-void btSolveL1 (const btScalar *L, btScalar *B, int n, int lskip1)
-{  
-  /* declare variables - Z matrix, p and q vectors, etc */
-  btScalar Z11,Z21,Z31,Z41,p1,q1,p2,p3,p4,*ex;
-  const btScalar *ell;
-  int lskip2,lskip3,i,j;
-  /* compute lskip values */
-  lskip2 = 2*lskip1;
-  lskip3 = 3*lskip1;
-  /* compute all 4 x 1 blocks of X */
-  for (i=0; i <= n-4; i+=4) {
-    /* compute all 4 x 1 block of X, from rows i..i+4-1 */
-    /* set the Z matrix to 0 */
-    Z11=0;
-    Z21=0;
-    Z31=0;
-    Z41=0;
-    ell = L + i*lskip1;
-    ex = B;
-    /* the inner loop that computes outer products and adds them to Z */
-    for (j=i-12; j >= 0; j -= 12) {
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[0];
-      p2=ell[lskip1];
-      p3=ell[lskip2];
-      p4=ell[lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[1];
-      q1=ex[1];
-      p2=ell[1+lskip1];
-      p3=ell[1+lskip2];
-      p4=ell[1+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[2];
-      q1=ex[2];
-      p2=ell[2+lskip1];
-      p3=ell[2+lskip2];
-      p4=ell[2+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[3];
-      q1=ex[3];
-      p2=ell[3+lskip1];
-      p3=ell[3+lskip2];
-      p4=ell[3+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[4];
-      q1=ex[4];
-      p2=ell[4+lskip1];
-      p3=ell[4+lskip2];
-      p4=ell[4+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[5];
-      q1=ex[5];
-      p2=ell[5+lskip1];
-      p3=ell[5+lskip2];
-      p4=ell[5+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[6];
-      q1=ex[6];
-      p2=ell[6+lskip1];
-      p3=ell[6+lskip2];
-      p4=ell[6+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[7];
-      q1=ex[7];
-      p2=ell[7+lskip1];
-      p3=ell[7+lskip2];
-      p4=ell[7+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[8];
-      q1=ex[8];
-      p2=ell[8+lskip1];
-      p3=ell[8+lskip2];
-      p4=ell[8+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[9];
-      q1=ex[9];
-      p2=ell[9+lskip1];
-      p3=ell[9+lskip2];
-      p4=ell[9+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[10];
-      q1=ex[10];
-      p2=ell[10+lskip1];
-      p3=ell[10+lskip2];
-      p4=ell[10+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* load p and q values */
-      p1=ell[11];
-      q1=ex[11];
-      p2=ell[11+lskip1];
-      p3=ell[11+lskip2];
-      p4=ell[11+lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* advance pointers */
-      ell += 12;
-      ex += 12;
-      /* end of inner loop */
-    }
-    /* compute left-over iterations */
-    j += 12;
-    for (; j > 0; j--) {
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[0];
-      p2=ell[lskip1];
-      p3=ell[lskip2];
-      p4=ell[lskip3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      Z21 += p2 * q1;
-      Z31 += p3 * q1;
-      Z41 += p4 * q1;
-      /* advance pointers */
-      ell += 1;
-      ex += 1;
-    }
-    /* finish computing the X(i) block */
-    Z11 = ex[0] - Z11;
-    ex[0] = Z11;
-    p1 = ell[lskip1];
-    Z21 = ex[1] - Z21 - p1*Z11;
-    ex[1] = Z21;
-    p1 = ell[lskip2];
-    p2 = ell[1+lskip2];
-    Z31 = ex[2] - Z31 - p1*Z11 - p2*Z21;
-    ex[2] = Z31;
-    p1 = ell[lskip3];
-    p2 = ell[1+lskip3];
-    p3 = ell[2+lskip3];
-    Z41 = ex[3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31;
-    ex[3] = Z41;
-    /* end of outer loop */
-  }
-  /* compute rows at end that are not a multiple of block size */
-  for (; i < n; i++) {
-    /* compute all 1 x 1 block of X, from rows i..i+1-1 */
-    /* set the Z matrix to 0 */
-    Z11=0;
-    ell = L + i*lskip1;
-    ex = B;
-    /* the inner loop that computes outer products and adds them to Z */
-    for (j=i-12; j >= 0; j -= 12) {
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[0];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[1];
-      q1=ex[1];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[2];
-      q1=ex[2];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[3];
-      q1=ex[3];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[4];
-      q1=ex[4];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[5];
-      q1=ex[5];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[6];
-      q1=ex[6];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[7];
-      q1=ex[7];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[8];
-      q1=ex[8];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[9];
-      q1=ex[9];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[10];
-      q1=ex[10];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* load p and q values */
-      p1=ell[11];
-      q1=ex[11];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* advance pointers */
-      ell += 12;
-      ex += 12;
-      /* end of inner loop */
-    }
-    /* compute left-over iterations */
-    j += 12;
-    for (; j > 0; j--) {
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[0];
-      /* compute outer product and add it to the Z matrix */
-      Z11 += p1 * q1;
-      /* advance pointers */
-      ell += 1;
-      ex += 1;
-    }
-    /* finish computing the X(i) block */
-    Z11 = ex[0] - Z11;
-    ex[0] = Z11;
-  }
+void btSolveL1(const btScalar *L, btScalar *B, int n, int lskip1)
+{
+	/* declare variables - Z matrix, p and q vectors, etc */
+	btScalar Z11, Z21, Z31, Z41, p1, q1, p2, p3, p4, *ex;
+	const btScalar *ell;
+	int lskip2, lskip3, i, j;
+	/* compute lskip values */
+	lskip2 = 2 * lskip1;
+	lskip3 = 3 * lskip1;
+	/* compute all 4 x 1 blocks of X */
+	for (i = 0; i <= n - 4; i += 4)
+	{
+		/* compute all 4 x 1 block of X, from rows i..i+4-1 */
+		/* set the Z matrix to 0 */
+		Z11 = 0;
+		Z21 = 0;
+		Z31 = 0;
+		Z41 = 0;
+		ell = L + i * lskip1;
+		ex = B;
+		/* the inner loop that computes outer products and adds them to Z */
+		for (j = i - 12; j >= 0; j -= 12)
+		{
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[0];
+			p2 = ell[lskip1];
+			p3 = ell[lskip2];
+			p4 = ell[lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[1];
+			q1 = ex[1];
+			p2 = ell[1 + lskip1];
+			p3 = ell[1 + lskip2];
+			p4 = ell[1 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[2];
+			q1 = ex[2];
+			p2 = ell[2 + lskip1];
+			p3 = ell[2 + lskip2];
+			p4 = ell[2 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[3];
+			q1 = ex[3];
+			p2 = ell[3 + lskip1];
+			p3 = ell[3 + lskip2];
+			p4 = ell[3 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[4];
+			q1 = ex[4];
+			p2 = ell[4 + lskip1];
+			p3 = ell[4 + lskip2];
+			p4 = ell[4 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[5];
+			q1 = ex[5];
+			p2 = ell[5 + lskip1];
+			p3 = ell[5 + lskip2];
+			p4 = ell[5 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[6];
+			q1 = ex[6];
+			p2 = ell[6 + lskip1];
+			p3 = ell[6 + lskip2];
+			p4 = ell[6 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[7];
+			q1 = ex[7];
+			p2 = ell[7 + lskip1];
+			p3 = ell[7 + lskip2];
+			p4 = ell[7 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[8];
+			q1 = ex[8];
+			p2 = ell[8 + lskip1];
+			p3 = ell[8 + lskip2];
+			p4 = ell[8 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[9];
+			q1 = ex[9];
+			p2 = ell[9 + lskip1];
+			p3 = ell[9 + lskip2];
+			p4 = ell[9 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[10];
+			q1 = ex[10];
+			p2 = ell[10 + lskip1];
+			p3 = ell[10 + lskip2];
+			p4 = ell[10 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* load p and q values */
+			p1 = ell[11];
+			q1 = ex[11];
+			p2 = ell[11 + lskip1];
+			p3 = ell[11 + lskip2];
+			p4 = ell[11 + lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* advance pointers */
+			ell += 12;
+			ex += 12;
+			/* end of inner loop */
+		}
+		/* compute left-over iterations */
+		j += 12;
+		for (; j > 0; j--)
+		{
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[0];
+			p2 = ell[lskip1];
+			p3 = ell[lskip2];
+			p4 = ell[lskip3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			Z21 += p2 * q1;
+			Z31 += p3 * q1;
+			Z41 += p4 * q1;
+			/* advance pointers */
+			ell += 1;
+			ex += 1;
+		}
+		/* finish computing the X(i) block */
+		Z11 = ex[0] - Z11;
+		ex[0] = Z11;
+		p1 = ell[lskip1];
+		Z21 = ex[1] - Z21 - p1 * Z11;
+		ex[1] = Z21;
+		p1 = ell[lskip2];
+		p2 = ell[1 + lskip2];
+		Z31 = ex[2] - Z31 - p1 * Z11 - p2 * Z21;
+		ex[2] = Z31;
+		p1 = ell[lskip3];
+		p2 = ell[1 + lskip3];
+		p3 = ell[2 + lskip3];
+		Z41 = ex[3] - Z41 - p1 * Z11 - p2 * Z21 - p3 * Z31;
+		ex[3] = Z41;
+		/* end of outer loop */
+	}
+	/* compute rows at end that are not a multiple of block size */
+	for (; i < n; i++)
+	{
+		/* compute all 1 x 1 block of X, from rows i..i+1-1 */
+		/* set the Z matrix to 0 */
+		Z11 = 0;
+		ell = L + i * lskip1;
+		ex = B;
+		/* the inner loop that computes outer products and adds them to Z */
+		for (j = i - 12; j >= 0; j -= 12)
+		{
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[0];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[1];
+			q1 = ex[1];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[2];
+			q1 = ex[2];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[3];
+			q1 = ex[3];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[4];
+			q1 = ex[4];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[5];
+			q1 = ex[5];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[6];
+			q1 = ex[6];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[7];
+			q1 = ex[7];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[8];
+			q1 = ex[8];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[9];
+			q1 = ex[9];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[10];
+			q1 = ex[10];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* load p and q values */
+			p1 = ell[11];
+			q1 = ex[11];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* advance pointers */
+			ell += 12;
+			ex += 12;
+			/* end of inner loop */
+		}
+		/* compute left-over iterations */
+		j += 12;
+		for (; j > 0; j--)
+		{
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[0];
+			/* compute outer product and add it to the Z matrix */
+			Z11 += p1 * q1;
+			/* advance pointers */
+			ell += 1;
+			ex += 1;
+		}
+		/* finish computing the X(i) block */
+		Z11 = ex[0] - Z11;
+		ex[0] = Z11;
+	}
 }
 
 /* solve L^T * x=b, with b containing 1 right hand side.
@@ -816,215 +829,218 @@ void btSolveL1 (const btScalar *L, btScalar *B, int n, int lskip1)
  * this processes blocks of 4.
  */
 
-void btSolveL1T (const btScalar *L, btScalar *B, int n, int lskip1)
-{  
-  /* declare variables - Z matrix, p and q vectors, etc */
-  btScalar Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex;
-  const btScalar *ell;
-  int lskip2,i,j;
-//  int lskip3;
-  /* special handling for L and B because we're solving L1 *transpose* */
-  L = L + (n-1)*(lskip1+1);
-  B = B + n-1;
-  lskip1 = -lskip1;
-  /* compute lskip values */
-  lskip2 = 2*lskip1;
-  //lskip3 = 3*lskip1;
-  /* compute all 4 x 1 blocks of X */
-  for (i=0; i <= n-4; i+=4) {
-    /* compute all 4 x 1 block of X, from rows i..i+4-1 */
-    /* set the Z matrix to 0 */
-    Z11=0;
-    Z21=0;
-    Z31=0;
-    Z41=0;
-    ell = L - i;
-    ex = B;
-    /* the inner loop that computes outer products and adds them to Z */
-    for (j=i-4; j >= 0; j -= 4) {
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[0];
-      p2=ell[-1];
-      p3=ell[-2];
-      p4=ell[-3];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      m21 = p2 * q1;
-      m31 = p3 * q1;
-      m41 = p4 * q1;
-      ell += lskip1;
-      Z11 += m11;
-      Z21 += m21;
-      Z31 += m31;
-      Z41 += m41;
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[-1];
-      p2=ell[-1];
-      p3=ell[-2];
-      p4=ell[-3];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      m21 = p2 * q1;
-      m31 = p3 * q1;
-      m41 = p4 * q1;
-      ell += lskip1;
-      Z11 += m11;
-      Z21 += m21;
-      Z31 += m31;
-      Z41 += m41;
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[-2];
-      p2=ell[-1];
-      p3=ell[-2];
-      p4=ell[-3];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      m21 = p2 * q1;
-      m31 = p3 * q1;
-      m41 = p4 * q1;
-      ell += lskip1;
-      Z11 += m11;
-      Z21 += m21;
-      Z31 += m31;
-      Z41 += m41;
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[-3];
-      p2=ell[-1];
-      p3=ell[-2];
-      p4=ell[-3];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      m21 = p2 * q1;
-      m31 = p3 * q1;
-      m41 = p4 * q1;
-      ell += lskip1;
-      ex -= 4;
-      Z11 += m11;
-      Z21 += m21;
-      Z31 += m31;
-      Z41 += m41;
-      /* end of inner loop */
-    }
-    /* compute left-over iterations */
-    j += 4;
-    for (; j > 0; j--) {
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[0];
-      p2=ell[-1];
-      p3=ell[-2];
-      p4=ell[-3];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      m21 = p2 * q1;
-      m31 = p3 * q1;
-      m41 = p4 * q1;
-      ell += lskip1;
-      ex -= 1;
-      Z11 += m11;
-      Z21 += m21;
-      Z31 += m31;
-      Z41 += m41;
-    }
-    /* finish computing the X(i) block */
-    Z11 = ex[0] - Z11;
-    ex[0] = Z11;
-    p1 = ell[-1];
-    Z21 = ex[-1] - Z21 - p1*Z11;
-    ex[-1] = Z21;
-    p1 = ell[-2];
-    p2 = ell[-2+lskip1];
-    Z31 = ex[-2] - Z31 - p1*Z11 - p2*Z21;
-    ex[-2] = Z31;
-    p1 = ell[-3];
-    p2 = ell[-3+lskip1];
-    p3 = ell[-3+lskip2];
-    Z41 = ex[-3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31;
-    ex[-3] = Z41;
-    /* end of outer loop */
-  }
-  /* compute rows at end that are not a multiple of block size */
-  for (; i < n; i++) {
-    /* compute all 1 x 1 block of X, from rows i..i+1-1 */
-    /* set the Z matrix to 0 */
-    Z11=0;
-    ell = L - i;
-    ex = B;
-    /* the inner loop that computes outer products and adds them to Z */
-    for (j=i-4; j >= 0; j -= 4) {
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[0];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      ell += lskip1;
-      Z11 += m11;
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[-1];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      ell += lskip1;
-      Z11 += m11;
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[-2];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      ell += lskip1;
-      Z11 += m11;
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[-3];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      ell += lskip1;
-      ex -= 4;
-      Z11 += m11;
-      /* end of inner loop */
-    }
-    /* compute left-over iterations */
-    j += 4;
-    for (; j > 0; j--) {
-      /* load p and q values */
-      p1=ell[0];
-      q1=ex[0];
-      /* compute outer product and add it to the Z matrix */
-      m11 = p1 * q1;
-      ell += lskip1;
-      ex -= 1;
-      Z11 += m11;
-    }
-    /* finish computing the X(i) block */
-    Z11 = ex[0] - Z11;
-    ex[0] = Z11;
-  }
+void btSolveL1T(const btScalar *L, btScalar *B, int n, int lskip1)
+{
+	/* declare variables - Z matrix, p and q vectors, etc */
+	btScalar Z11, m11, Z21, m21, Z31, m31, Z41, m41, p1, q1, p2, p3, p4, *ex;
+	const btScalar *ell;
+	int lskip2, i, j;
+	//  int lskip3;
+	/* special handling for L and B because we're solving L1 *transpose* */
+	L = L + (n - 1) * (lskip1 + 1);
+	B = B + n - 1;
+	lskip1 = -lskip1;
+	/* compute lskip values */
+	lskip2 = 2 * lskip1;
+	//lskip3 = 3*lskip1;
+	/* compute all 4 x 1 blocks of X */
+	for (i = 0; i <= n - 4; i += 4)
+	{
+		/* compute all 4 x 1 block of X, from rows i..i+4-1 */
+		/* set the Z matrix to 0 */
+		Z11 = 0;
+		Z21 = 0;
+		Z31 = 0;
+		Z41 = 0;
+		ell = L - i;
+		ex = B;
+		/* the inner loop that computes outer products and adds them to Z */
+		for (j = i - 4; j >= 0; j -= 4)
+		{
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[0];
+			p2 = ell[-1];
+			p3 = ell[-2];
+			p4 = ell[-3];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m31 = p3 * q1;
+			m41 = p4 * q1;
+			ell += lskip1;
+			Z11 += m11;
+			Z21 += m21;
+			Z31 += m31;
+			Z41 += m41;
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[-1];
+			p2 = ell[-1];
+			p3 = ell[-2];
+			p4 = ell[-3];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m31 = p3 * q1;
+			m41 = p4 * q1;
+			ell += lskip1;
+			Z11 += m11;
+			Z21 += m21;
+			Z31 += m31;
+			Z41 += m41;
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[-2];
+			p2 = ell[-1];
+			p3 = ell[-2];
+			p4 = ell[-3];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m31 = p3 * q1;
+			m41 = p4 * q1;
+			ell += lskip1;
+			Z11 += m11;
+			Z21 += m21;
+			Z31 += m31;
+			Z41 += m41;
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[-3];
+			p2 = ell[-1];
+			p3 = ell[-2];
+			p4 = ell[-3];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m31 = p3 * q1;
+			m41 = p4 * q1;
+			ell += lskip1;
+			ex -= 4;
+			Z11 += m11;
+			Z21 += m21;
+			Z31 += m31;
+			Z41 += m41;
+			/* end of inner loop */
+		}
+		/* compute left-over iterations */
+		j += 4;
+		for (; j > 0; j--)
+		{
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[0];
+			p2 = ell[-1];
+			p3 = ell[-2];
+			p4 = ell[-3];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			m21 = p2 * q1;
+			m31 = p3 * q1;
+			m41 = p4 * q1;
+			ell += lskip1;
+			ex -= 1;
+			Z11 += m11;
+			Z21 += m21;
+			Z31 += m31;
+			Z41 += m41;
+		}
+		/* finish computing the X(i) block */
+		Z11 = ex[0] - Z11;
+		ex[0] = Z11;
+		p1 = ell[-1];
+		Z21 = ex[-1] - Z21 - p1 * Z11;
+		ex[-1] = Z21;
+		p1 = ell[-2];
+		p2 = ell[-2 + lskip1];
+		Z31 = ex[-2] - Z31 - p1 * Z11 - p2 * Z21;
+		ex[-2] = Z31;
+		p1 = ell[-3];
+		p2 = ell[-3 + lskip1];
+		p3 = ell[-3 + lskip2];
+		Z41 = ex[-3] - Z41 - p1 * Z11 - p2 * Z21 - p3 * Z31;
+		ex[-3] = Z41;
+		/* end of outer loop */
+	}
+	/* compute rows at end that are not a multiple of block size */
+	for (; i < n; i++)
+	{
+		/* compute all 1 x 1 block of X, from rows i..i+1-1 */
+		/* set the Z matrix to 0 */
+		Z11 = 0;
+		ell = L - i;
+		ex = B;
+		/* the inner loop that computes outer products and adds them to Z */
+		for (j = i - 4; j >= 0; j -= 4)
+		{
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[0];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			ell += lskip1;
+			Z11 += m11;
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[-1];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			ell += lskip1;
+			Z11 += m11;
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[-2];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			ell += lskip1;
+			Z11 += m11;
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[-3];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			ell += lskip1;
+			ex -= 4;
+			Z11 += m11;
+			/* end of inner loop */
+		}
+		/* compute left-over iterations */
+		j += 4;
+		for (; j > 0; j--)
+		{
+			/* load p and q values */
+			p1 = ell[0];
+			q1 = ex[0];
+			/* compute outer product and add it to the Z matrix */
+			m11 = p1 * q1;
+			ell += lskip1;
+			ex -= 1;
+			Z11 += m11;
+		}
+		/* finish computing the X(i) block */
+		Z11 = ex[0] - Z11;
+		ex[0] = Z11;
+	}
 }
 
-
-
-void btVectorScale (btScalar *a, const btScalar *d, int n)
+void btVectorScale(btScalar *a, const btScalar *d, int n)
 {
-  btAssert (a && d && n >= 0);
-  for (int i=0; i<n; i++) {
-    a[i] *= d[i];
-  }
+	btAssert(a && d && n >= 0);
+	for (int i = 0; i < n; i++)
+	{
+		a[i] *= d[i];
+	}
 }
 
-void btSolveLDLT (const btScalar *L, const btScalar *d, btScalar *b, int n, int nskip)
+void btSolveLDLT(const btScalar *L, const btScalar *d, btScalar *b, int n, int nskip)
 {
-  btAssert (L && d && b && n > 0 && nskip >= n);
-  btSolveL1 (L,b,n,nskip);
-  btVectorScale (b,d,n);
-  btSolveL1T (L,b,n,nskip);
+	btAssert(L && d && b && n > 0 && nskip >= n);
+	btSolveL1(L, b, n, nskip);
+	btVectorScale(b, d, n);
+	btSolveL1T(L, b, n, nskip);
 }
 
-
-
 //***************************************************************************
 
 // swap row/column i1 with i2 in the n*n matrix A. the leading dimension of
@@ -1033,124 +1049,129 @@ void btSolveLDLT (const btScalar *L, const btScalar *d, btScalar *b, int n, int
 // rows will be swapped by exchanging row pointers. otherwise the data will
 // be copied.
 
-static void btSwapRowsAndCols (BTATYPE A, int n, int i1, int i2, int nskip, 
-  int do_fast_row_swaps)
+static void btSwapRowsAndCols(BTATYPE A, int n, int i1, int i2, int nskip,
+							  int do_fast_row_swaps)
 {
-  btAssert (A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n &&
-    nskip >= n && i1 < i2);
-
-# ifdef BTROWPTRS
-  btScalar *A_i1 = A[i1];
-  btScalar *A_i2 = A[i2];
-  for (int i=i1+1; i<i2; ++i) {
-    btScalar *A_i_i1 = A[i] + i1;
-    A_i1[i] = *A_i_i1;
-    *A_i_i1 = A_i2[i];
-  }
-  A_i1[i2] = A_i1[i1];
-  A_i1[i1] = A_i2[i1];
-  A_i2[i1] = A_i2[i2];
-  // swap rows, by swapping row pointers
-  if (do_fast_row_swaps) {
-    A[i1] = A_i2;
-    A[i2] = A_i1;
-  }
-  else {
-    // Only swap till i2 column to match A plain storage variant.
-    for (int k = 0; k <= i2; ++k) {
-      btScalar tmp = A_i1[k];
-      A_i1[k] = A_i2[k];
-      A_i2[k] = tmp;
-    }
-  }
-  // swap columns the hard way
-  for (int j=i2+1; j<n; ++j) {
-    btScalar *A_j = A[j];
-    btScalar tmp = A_j[i1];
-    A_j[i1] = A_j[i2];
-    A_j[i2] = tmp;
-  }
-# else
-  btScalar *A_i1 = A+i1*nskip;
-  btScalar *A_i2 = A+i2*nskip;
-  for (int k = 0; k < i1; ++k) {
-    btScalar tmp = A_i1[k];
-    A_i1[k] = A_i2[k];
-    A_i2[k] = tmp;
-  }
-  btScalar *A_i = A_i1 + nskip;
-  for (int i=i1+1; i<i2; A_i+=nskip, ++i) {
-    btScalar tmp = A_i2[i];
-    A_i2[i] = A_i[i1];
-    A_i[i1] = tmp;
-  }
-  {
-    btScalar tmp = A_i1[i1];
-    A_i1[i1] = A_i2[i2];
-    A_i2[i2] = tmp;
-  }
-  btScalar *A_j = A_i2 + nskip;
-  for (int j=i2+1; j<n; A_j+=nskip, ++j) {
-    btScalar tmp = A_j[i1];
-    A_j[i1] = A_j[i2];
-    A_j[i2] = tmp;
-  }
-# endif
-}
+	btAssert(A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n &&
+			 nskip >= n && i1 < i2);
 
+#ifdef BTROWPTRS
+	btScalar *A_i1 = A[i1];
+	btScalar *A_i2 = A[i2];
+	for (int i = i1 + 1; i < i2; ++i)
+	{
+		btScalar *A_i_i1 = A[i] + i1;
+		A_i1[i] = *A_i_i1;
+		*A_i_i1 = A_i2[i];
+	}
+	A_i1[i2] = A_i1[i1];
+	A_i1[i1] = A_i2[i1];
+	A_i2[i1] = A_i2[i2];
+	// swap rows, by swapping row pointers
+	if (do_fast_row_swaps)
+	{
+		A[i1] = A_i2;
+		A[i2] = A_i1;
+	}
+	else
+	{
+		// Only swap till i2 column to match A plain storage variant.
+		for (int k = 0; k <= i2; ++k)
+		{
+			btScalar tmp = A_i1[k];
+			A_i1[k] = A_i2[k];
+			A_i2[k] = tmp;
+		}
+	}
+	// swap columns the hard way
+	for (int j = i2 + 1; j < n; ++j)
+	{
+		btScalar *A_j = A[j];
+		btScalar tmp = A_j[i1];
+		A_j[i1] = A_j[i2];
+		A_j[i2] = tmp;
+	}
+#else
+	btScalar *A_i1 = A + i1 * nskip;
+	btScalar *A_i2 = A + i2 * nskip;
+	for (int k = 0; k < i1; ++k)
+	{
+		btScalar tmp = A_i1[k];
+		A_i1[k] = A_i2[k];
+		A_i2[k] = tmp;
+	}
+	btScalar *A_i = A_i1 + nskip;
+	for (int i = i1 + 1; i < i2; A_i += nskip, ++i)
+	{
+		btScalar tmp = A_i2[i];
+		A_i2[i] = A_i[i1];
+		A_i[i1] = tmp;
+	}
+	{
+		btScalar tmp = A_i1[i1];
+		A_i1[i1] = A_i2[i2];
+		A_i2[i2] = tmp;
+	}
+	btScalar *A_j = A_i2 + nskip;
+	for (int j = i2 + 1; j < n; A_j += nskip, ++j)
+	{
+		btScalar tmp = A_j[i1];
+		A_j[i1] = A_j[i2];
+		A_j[i2] = tmp;
+	}
+#endif
+}
 
 // swap two indexes in the n*n LCP problem. i1 must be <= i2.
 
-static void btSwapProblem (BTATYPE A, btScalar *x, btScalar *b, btScalar *w, btScalar *lo,
-                         btScalar *hi, int *p, bool *state, int *findex,
-                         int n, int i1, int i2, int nskip,
-                         int do_fast_row_swaps)
+static void btSwapProblem(BTATYPE A, btScalar *x, btScalar *b, btScalar *w, btScalar *lo,
+						  btScalar *hi, int *p, bool *state, int *findex,
+						  int n, int i1, int i2, int nskip,
+						  int do_fast_row_swaps)
 {
-  btScalar tmpr;
-  int tmpi;
-  bool tmpb;
-  btAssert (n>0 && i1 >=0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n && i1 <= i2);
-  if (i1==i2) return;
-  
-  btSwapRowsAndCols (A,n,i1,i2,nskip,do_fast_row_swaps);
-  
-  tmpr = x[i1];
-  x[i1] = x[i2];
-  x[i2] = tmpr;
-  
-  tmpr = b[i1];
-  b[i1] = b[i2];
-  b[i2] = tmpr;
-  
-  tmpr = w[i1];
-  w[i1] = w[i2];
-  w[i2] = tmpr;
-  
-  tmpr = lo[i1];
-  lo[i1] = lo[i2];
-  lo[i2] = tmpr;
-
-  tmpr = hi[i1];
-  hi[i1] = hi[i2];
-  hi[i2] = tmpr;
-
-  tmpi = p[i1];
-  p[i1] = p[i2];
-  p[i2] = tmpi;
-
-  tmpb = state[i1];
-  state[i1] = state[i2];
-  state[i2] = tmpb;
-
-  if (findex) {
-    tmpi = findex[i1];
-    findex[i1] = findex[i2];
-    findex[i2] = tmpi;
-  }
-}
+	btScalar tmpr;
+	int tmpi;
+	bool tmpb;
+	btAssert(n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n && i1 <= i2);
+	if (i1 == i2) return;
+
+	btSwapRowsAndCols(A, n, i1, i2, nskip, do_fast_row_swaps);
+
+	tmpr = x[i1];
+	x[i1] = x[i2];
+	x[i2] = tmpr;
 
+	tmpr = b[i1];
+	b[i1] = b[i2];
+	b[i2] = tmpr;
 
+	tmpr = w[i1];
+	w[i1] = w[i2];
+	w[i2] = tmpr;
 
+	tmpr = lo[i1];
+	lo[i1] = lo[i2];
+	lo[i2] = tmpr;
+
+	tmpr = hi[i1];
+	hi[i1] = hi[i2];
+	hi[i2] = tmpr;
+
+	tmpi = p[i1];
+	p[i1] = p[i2];
+	p[i2] = tmpi;
+
+	tmpb = state[i1];
+	state[i1] = state[i2];
+	state[i2] = tmpb;
+
+	if (findex)
+	{
+		tmpi = findex[i1];
+		findex[i1] = findex[i2];
+		findex[i2] = tmpi;
+	}
+}
 
 //***************************************************************************
 // btLCP manipulator object. this represents an n*n LCP problem.
@@ -1186,79 +1207,88 @@ static void btSwapProblem (BTATYPE A, btScalar *x, btScalar *b, btScalar *w, btS
 
 #ifdef btLCP_FAST
 
-struct btLCP 
+struct btLCP
 {
 	const int m_n;
 	const int m_nskip;
 	int m_nub;
-	int m_nC, m_nN;				// size of each index set
-	BTATYPE const m_A;				// A rows
-	btScalar *const m_x, * const m_b, *const m_w, *const m_lo,* const m_hi;	// permuted LCP problem data
-	btScalar *const m_L, *const m_d;				// L*D*L' factorization of set C
+	int m_nC, m_nN;                                                         // size of each index set
+	BTATYPE const m_A;                                                      // A rows
+	btScalar *const m_x, *const m_b, *const m_w, *const m_lo, *const m_hi;  // permuted LCP problem data
+	btScalar *const m_L, *const m_d;                                        // L*D*L' factorization of set C
 	btScalar *const m_Dell, *const m_ell, *const m_tmp;
 	bool *const m_state;
 	int *const m_findex, *const m_p, *const m_C;
 
-	btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w,
-		btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d,
-		btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
-		bool *_state, int *_findex, int *p, int *c, btScalar **Arows);
+	btLCP(int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w,
+		  btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d,
+		  btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
+		  bool *_state, int *_findex, int *p, int *c, btScalar **Arows);
 	int getNub() const { return m_nub; }
-	void transfer_i_to_C (int i);
-	void transfer_i_to_N (int i) { m_nN++; }			// because we can assume C and N span 1:i-1
-	void transfer_i_from_N_to_C (int i);
-	void transfer_i_from_C_to_N (int i, btAlignedObjectArray<btScalar>& scratch);
+	void transfer_i_to_C(int i);
+	void transfer_i_to_N(int i) { m_nN++; }  // because we can assume C and N span 1:i-1
+	void transfer_i_from_N_to_C(int i);
+	void transfer_i_from_C_to_N(int i, btAlignedObjectArray<btScalar> &scratch);
 	int numC() const { return m_nC; }
 	int numN() const { return m_nN; }
-	int indexC (int i) const { return i; }
-	int indexN (int i) const { return i+m_nC; }
-	btScalar Aii (int i) const  { return BTAROW(i)[i]; }
-	btScalar AiC_times_qC (int i, btScalar *q) const { return btLargeDot (BTAROW(i), q, m_nC); }
-	btScalar AiN_times_qN (int i, btScalar *q) const { return btLargeDot (BTAROW(i)+m_nC, q+m_nC, m_nN); }
-	void pN_equals_ANC_times_qC (btScalar *p, btScalar *q);
-	void pN_plusequals_ANi (btScalar *p, int i, int sign=1);
-	void pC_plusequals_s_times_qC (btScalar *p, btScalar s, btScalar *q);
-	void pN_plusequals_s_times_qN (btScalar *p, btScalar s, btScalar *q);
-	void solve1 (btScalar *a, int i, int dir=1, int only_transfer=0);
+	int indexC(int i) const { return i; }
+	int indexN(int i) const { return i + m_nC; }
+	btScalar Aii(int i) const { return BTAROW(i)[i]; }
+	btScalar AiC_times_qC(int i, btScalar *q) const { return btLargeDot(BTAROW(i), q, m_nC); }
+	btScalar AiN_times_qN(int i, btScalar *q) const { return btLargeDot(BTAROW(i) + m_nC, q + m_nC, m_nN); }
+	void pN_equals_ANC_times_qC(btScalar *p, btScalar *q);
+	void pN_plusequals_ANi(btScalar *p, int i, int sign = 1);
+	void pC_plusequals_s_times_qC(btScalar *p, btScalar s, btScalar *q);
+	void pN_plusequals_s_times_qN(btScalar *p, btScalar s, btScalar *q);
+	void solve1(btScalar *a, int i, int dir = 1, int only_transfer = 0);
 	void unpermute();
 };
 
-
-btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w,
-            btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d,
-            btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
-            bool *_state, int *_findex, int *p, int *c, btScalar **Arows):
-  m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0),
-# ifdef BTROWPTRS
-  m_A(Arows),
+btLCP::btLCP(int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w,
+			 btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d,
+			 btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
+			 bool *_state, int *_findex, int *p, int *c, btScalar **Arows) : m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0),
+#ifdef BTROWPTRS
+																			 m_A(Arows),
 #else
-  m_A(_Adata),
+																			 m_A(_Adata),
 #endif
-  m_x(_x), m_b(_b), m_w(_w), m_lo(_lo), m_hi(_hi),
-  m_L(l), m_d(_d), m_Dell(_Dell), m_ell(_ell), m_tmp(_tmp),
-  m_state(_state), m_findex(_findex), m_p(p), m_C(c)
+																			 m_x(_x),
+																			 m_b(_b),
+																			 m_w(_w),
+																			 m_lo(_lo),
+																			 m_hi(_hi),
+																			 m_L(l),
+																			 m_d(_d),
+																			 m_Dell(_Dell),
+																			 m_ell(_ell),
+																			 m_tmp(_tmp),
+																			 m_state(_state),
+																			 m_findex(_findex),
+																			 m_p(p),
+																			 m_C(c)
 {
-  {
-    btSetZero (m_x,m_n);
-  }
+	{
+		btSetZero(m_x, m_n);
+	}
 
-  {
-# ifdef BTROWPTRS
-    // make matrix row pointers
-    btScalar *aptr = _Adata;
-    BTATYPE A = m_A;
-    const int n = m_n, nskip = m_nskip;
-    for (int k=0; k<n; aptr+=nskip, ++k) A[k] = aptr;
-# endif
-  }
+	{
+#ifdef BTROWPTRS
+		// make matrix row pointers
+		btScalar *aptr = _Adata;
+		BTATYPE A = m_A;
+		const int n = m_n, nskip = m_nskip;
+		for (int k = 0; k < n; aptr += nskip, ++k) A[k] = aptr;
+#endif
+	}
 
-  {
-    int *p = m_p;
-    const int n = m_n;
-    for (int k=0; k<n; ++k) p[k]=k;		// initially unpermuted
-  }
+	{
+		int *p = m_p;
+		const int n = m_n;
+		for (int k = 0; k < n; ++k) p[k] = k;  // initially unpermuted
+	}
 
-  /*
+	/*
   // for testing, we can do some random swaps in the area i > nub
   {
     const int n = m_n;
@@ -1277,63 +1307,69 @@ btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btSc
   }
   */
 
-  // permute the problem so that *all* the unbounded variables are at the
-  // start, i.e. look for unbounded variables not included in `nub'. we can
-  // potentially push up `nub' this way and get a bigger initial factorization.
-  // note that when we swap rows/cols here we must not just swap row pointers,
-  // as the initial factorization relies on the data being all in one chunk.
-  // variables that have findex >= 0 are *not* considered to be unbounded even
-  // if lo=-inf and hi=inf - this is because these limits may change during the
-  // solution process.
-
-  {
-    int *findex = m_findex;
-    btScalar *lo = m_lo, *hi = m_hi;
-    const int n = m_n;
-    for (int k = m_nub; k<n; ++k) {
-      if (findex && findex[k] >= 0) continue;
-      if (lo[k]==-BT_INFINITY && hi[k]==BT_INFINITY) {
-        btSwapProblem (m_A,m_x,m_b,m_w,lo,hi,m_p,m_state,findex,n,m_nub,k,m_nskip,0);
-        m_nub++;
-      }
-    }
-  }
-
-  // if there are unbounded variables at the start, factorize A up to that
-  // point and solve for x. this puts all indexes 0..nub-1 into C.
-  if (m_nub > 0) {
-    const int nub = m_nub;
-    {
-      btScalar *Lrow = m_L;
-      const int nskip = m_nskip;
-      for (int j=0; j<nub; Lrow+=nskip, ++j) memcpy(Lrow,BTAROW(j),(j+1)*sizeof(btScalar));
-    }
-    btFactorLDLT (m_L,m_d,nub,m_nskip);
-    memcpy (m_x,m_b,nub*sizeof(btScalar));
-    btSolveLDLT (m_L,m_d,m_x,nub,m_nskip);
-    btSetZero (m_w,nub);
-    {
-      int *C = m_C;
-      for (int k=0; k<nub; ++k) C[k] = k;
-    }
-    m_nC = nub;
-  }
-
-  // permute the indexes > nub such that all findex variables are at the end
-  if (m_findex) {
-    const int nub = m_nub;
-    int *findex = m_findex;
-    int num_at_end = 0;
-    for (int k=m_n-1; k >= nub; k--) {
-      if (findex[k] >= 0) {
-        btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,findex,m_n,k,m_n-1-num_at_end,m_nskip,1);
-        num_at_end++;
-      }
-    }
-  }
+	// permute the problem so that *all* the unbounded variables are at the
+	// start, i.e. look for unbounded variables not included in `nub'. we can
+	// potentially push up `nub' this way and get a bigger initial factorization.
+	// note that when we swap rows/cols here we must not just swap row pointers,
+	// as the initial factorization relies on the data being all in one chunk.
+	// variables that have findex >= 0 are *not* considered to be unbounded even
+	// if lo=-inf and hi=inf - this is because these limits may change during the
+	// solution process.
 
-  // print info about indexes
-  /*
+	{
+		int *findex = m_findex;
+		btScalar *lo = m_lo, *hi = m_hi;
+		const int n = m_n;
+		for (int k = m_nub; k < n; ++k)
+		{
+			if (findex && findex[k] >= 0) continue;
+			if (lo[k] == -BT_INFINITY && hi[k] == BT_INFINITY)
+			{
+				btSwapProblem(m_A, m_x, m_b, m_w, lo, hi, m_p, m_state, findex, n, m_nub, k, m_nskip, 0);
+				m_nub++;
+			}
+		}
+	}
+
+	// if there are unbounded variables at the start, factorize A up to that
+	// point and solve for x. this puts all indexes 0..nub-1 into C.
+	if (m_nub > 0)
+	{
+		const int nub = m_nub;
+		{
+			btScalar *Lrow = m_L;
+			const int nskip = m_nskip;
+			for (int j = 0; j < nub; Lrow += nskip, ++j) memcpy(Lrow, BTAROW(j), (j + 1) * sizeof(btScalar));
+		}
+		btFactorLDLT(m_L, m_d, nub, m_nskip);
+		memcpy(m_x, m_b, nub * sizeof(btScalar));
+		btSolveLDLT(m_L, m_d, m_x, nub, m_nskip);
+		btSetZero(m_w, nub);
+		{
+			int *C = m_C;
+			for (int k = 0; k < nub; ++k) C[k] = k;
+		}
+		m_nC = nub;
+	}
+
+	// permute the indexes > nub such that all findex variables are at the end
+	if (m_findex)
+	{
+		const int nub = m_nub;
+		int *findex = m_findex;
+		int num_at_end = 0;
+		for (int k = m_n - 1; k >= nub; k--)
+		{
+			if (findex[k] >= 0)
+			{
+				btSwapProblem(m_A, m_x, m_b, m_w, m_lo, m_hi, m_p, m_state, findex, m_n, k, m_n - 1 - num_at_end, m_nskip, 1);
+				num_at_end++;
+			}
+		}
+	}
+
+	// print info about indexes
+	/*
   {
     const int n = m_n;
     const int nub = m_nub;
@@ -1347,734 +1383,776 @@ btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btSc
   */
 }
 
-
-void btLCP::transfer_i_to_C (int i)
+void btLCP::transfer_i_to_C(int i)
 {
-  {
-    if (m_nC > 0) {
-      // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C))
-      {
-        const int nC = m_nC;
-        btScalar *const Ltgt = m_L + nC*m_nskip, *ell = m_ell;
-        for (int j=0; j<nC; ++j) Ltgt[j] = ell[j];
-      }
-      const int nC = m_nC;
-      m_d[nC] = btRecip (BTAROW(i)[i] - btLargeDot(m_ell,m_Dell,nC));
-    }
-    else {
-      m_d[0] = btRecip (BTAROW(i)[i]);
-    }
-
-    btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,m_nC,i,m_nskip,1);
+	{
+		if (m_nC > 0)
+		{
+			// ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C))
+			{
+				const int nC = m_nC;
+				btScalar *const Ltgt = m_L + nC * m_nskip, *ell = m_ell;
+				for (int j = 0; j < nC; ++j) Ltgt[j] = ell[j];
+			}
+			const int nC = m_nC;
+			m_d[nC] = btRecip(BTAROW(i)[i] - btLargeDot(m_ell, m_Dell, nC));
+		}
+		else
+		{
+			m_d[0] = btRecip(BTAROW(i)[i]);
+		}
 
-    const int nC = m_nC;
-    m_C[nC] = nC;
-    m_nC = nC + 1; // nC value is outdated after this line
-  }
+		btSwapProblem(m_A, m_x, m_b, m_w, m_lo, m_hi, m_p, m_state, m_findex, m_n, m_nC, i, m_nskip, 1);
 
+		const int nC = m_nC;
+		m_C[nC] = nC;
+		m_nC = nC + 1;  // nC value is outdated after this line
+	}
 }
 
-
-void btLCP::transfer_i_from_N_to_C (int i)
+void btLCP::transfer_i_from_N_to_C(int i)
 {
-  {
-    if (m_nC > 0) {
-      {
-        btScalar *const aptr = BTAROW(i);
-        btScalar *Dell = m_Dell;
-        const int *C = m_C;
-#   ifdef BTNUB_OPTIMIZATIONS
-        // if nub>0, initial part of aptr unpermuted
-        const int nub = m_nub;
-        int j=0;
-        for ( ; j<nub; ++j) Dell[j] = aptr[j];
-        const int nC = m_nC;
-        for ( ; j<nC; ++j) Dell[j] = aptr[C[j]];
-#   else
-        const int nC = m_nC;
-        for (int j=0; j<nC; ++j) Dell[j] = aptr[C[j]];
-#   endif
-      }
-      btSolveL1 (m_L,m_Dell,m_nC,m_nskip);
-      {
-        const int nC = m_nC;
-        btScalar *const Ltgt = m_L + nC*m_nskip;
-        btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d;
-        for (int j=0; j<nC; ++j) Ltgt[j] = ell[j] = Dell[j] * d[j];
-      }
-      const int nC = m_nC;
-      m_d[nC] = btRecip (BTAROW(i)[i] - btLargeDot(m_ell,m_Dell,nC));
-    }
-    else {
-      m_d[0] = btRecip (BTAROW(i)[i]);
-    }
-
-    btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,m_nC,i,m_nskip,1);
-
-    const int nC = m_nC;
-    m_C[nC] = nC;
-    m_nN--;
-    m_nC = nC + 1; // nC value is outdated after this line
-  }
-
-  // @@@ TO DO LATER
-  // if we just finish here then we'll go back and re-solve for
-  // delta_x. but actually we can be more efficient and incrementally
-  // update delta_x here. but if we do this, we wont have ell and Dell
-  // to use in updating the factorization later.
-
+	{
+		if (m_nC > 0)
+		{
+			{
+				btScalar *const aptr = BTAROW(i);
+				btScalar *Dell = m_Dell;
+				const int *C = m_C;
+#ifdef BTNUB_OPTIMIZATIONS
+				// if nub>0, initial part of aptr unpermuted
+				const int nub = m_nub;
+				int j = 0;
+				for (; j < nub; ++j) Dell[j] = aptr[j];
+				const int nC = m_nC;
+				for (; j < nC; ++j) Dell[j] = aptr[C[j]];
+#else
+				const int nC = m_nC;
+				for (int j = 0; j < nC; ++j) Dell[j] = aptr[C[j]];
+#endif
+			}
+			btSolveL1(m_L, m_Dell, m_nC, m_nskip);
+			{
+				const int nC = m_nC;
+				btScalar *const Ltgt = m_L + nC * m_nskip;
+				btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d;
+				for (int j = 0; j < nC; ++j) Ltgt[j] = ell[j] = Dell[j] * d[j];
+			}
+			const int nC = m_nC;
+			m_d[nC] = btRecip(BTAROW(i)[i] - btLargeDot(m_ell, m_Dell, nC));
+		}
+		else
+		{
+			m_d[0] = btRecip(BTAROW(i)[i]);
+		}
+
+		btSwapProblem(m_A, m_x, m_b, m_w, m_lo, m_hi, m_p, m_state, m_findex, m_n, m_nC, i, m_nskip, 1);
+
+		const int nC = m_nC;
+		m_C[nC] = nC;
+		m_nN--;
+		m_nC = nC + 1;  // nC value is outdated after this line
+	}
+
+	// @@@ TO DO LATER
+	// if we just finish here then we'll go back and re-solve for
+	// delta_x. but actually we can be more efficient and incrementally
+	// update delta_x here. but if we do this, we wont have ell and Dell
+	// to use in updating the factorization later.
 }
 
-void btRemoveRowCol (btScalar *A, int n, int nskip, int r)
+void btRemoveRowCol(btScalar *A, int n, int nskip, int r)
 {
-  btAssert(A && n > 0 && nskip >= n && r >= 0 && r < n);
-  if (r >= n-1) return;
-  if (r > 0) {
-    {
-      const size_t move_size = (n-r-1)*sizeof(btScalar);
-      btScalar *Adst = A + r;
-      for (int i=0; i<r; Adst+=nskip,++i) {
-        btScalar *Asrc = Adst + 1;
-        memmove (Adst,Asrc,move_size);
-      }
-    }
-    {
-      const size_t cpy_size = r*sizeof(btScalar);
-      btScalar *Adst = A + r * nskip;
-      for (int i=r; i<(n-1); ++i) {
-        btScalar *Asrc = Adst + nskip;
-        memcpy (Adst,Asrc,cpy_size);
-        Adst = Asrc;
-      }
-    }
-  }
-  {
-    const size_t cpy_size = (n-r-1)*sizeof(btScalar);
-    btScalar *Adst = A + r * (nskip + 1);
-    for (int i=r; i<(n-1); ++i) {
-      btScalar *Asrc = Adst + (nskip + 1);
-      memcpy (Adst,Asrc,cpy_size);
-      Adst = Asrc - 1;
-    }
-  }
+	btAssert(A && n > 0 && nskip >= n && r >= 0 && r < n);
+	if (r >= n - 1) return;
+	if (r > 0)
+	{
+		{
+			const size_t move_size = (n - r - 1) * sizeof(btScalar);
+			btScalar *Adst = A + r;
+			for (int i = 0; i < r; Adst += nskip, ++i)
+			{
+				btScalar *Asrc = Adst + 1;
+				memmove(Adst, Asrc, move_size);
+			}
+		}
+		{
+			const size_t cpy_size = r * sizeof(btScalar);
+			btScalar *Adst = A + r * nskip;
+			for (int i = r; i < (n - 1); ++i)
+			{
+				btScalar *Asrc = Adst + nskip;
+				memcpy(Adst, Asrc, cpy_size);
+				Adst = Asrc;
+			}
+		}
+	}
+	{
+		const size_t cpy_size = (n - r - 1) * sizeof(btScalar);
+		btScalar *Adst = A + r * (nskip + 1);
+		for (int i = r; i < (n - 1); ++i)
+		{
+			btScalar *Asrc = Adst + (nskip + 1);
+			memcpy(Adst, Asrc, cpy_size);
+			Adst = Asrc - 1;
+		}
+	}
 }
 
+void btLDLTAddTL(btScalar *L, btScalar *d, const btScalar *a, int n, int nskip, btAlignedObjectArray<btScalar> &scratch)
+{
+	btAssert(L && d && a && n > 0 && nskip >= n);
 
+	if (n < 2) return;
+	scratch.resize(2 * nskip);
+	btScalar *W1 = &scratch[0];
 
+	btScalar *W2 = W1 + nskip;
 
-void btLDLTAddTL (btScalar *L, btScalar *d, const btScalar *a, int n, int nskip, btAlignedObjectArray<btScalar>& scratch)
-{
-  btAssert (L && d && a && n > 0 && nskip >= n);
-
-  if (n < 2) return;
-  scratch.resize(2*nskip);
-  btScalar *W1 = &scratch[0];
-  
-  btScalar *W2 = W1 + nskip;
-
-  W1[0] = btScalar(0.0);
-  W2[0] = btScalar(0.0);
-  for (int j=1; j<n; ++j) {
-    W1[j] = W2[j] = (btScalar) (a[j] * SIMDSQRT12);
-  }
-  btScalar W11 = (btScalar) ((btScalar(0.5)*a[0]+1)*SIMDSQRT12);
-  btScalar W21 = (btScalar) ((btScalar(0.5)*a[0]-1)*SIMDSQRT12);
-
-  btScalar alpha1 = btScalar(1.0);
-  btScalar alpha2 = btScalar(1.0);
+	W1[0] = btScalar(0.0);
+	W2[0] = btScalar(0.0);
+	for (int j = 1; j < n; ++j)
+	{
+		W1[j] = W2[j] = (btScalar)(a[j] * SIMDSQRT12);
+	}
+	btScalar W11 = (btScalar)((btScalar(0.5) * a[0] + 1) * SIMDSQRT12);
+	btScalar W21 = (btScalar)((btScalar(0.5) * a[0] - 1) * SIMDSQRT12);
 
-  {
-    btScalar dee = d[0];
-    btScalar alphanew = alpha1 + (W11*W11)*dee;
-    btAssert(alphanew != btScalar(0.0));
-    dee /= alphanew;
-    btScalar gamma1 = W11 * dee;
-    dee *= alpha1;
-    alpha1 = alphanew;
-    alphanew = alpha2 - (W21*W21)*dee;
-    dee /= alphanew;
-    //btScalar gamma2 = W21 * dee;
-    alpha2 = alphanew;
-    btScalar k1 = btScalar(1.0) - W21*gamma1;
-    btScalar k2 = W21*gamma1*W11 - W21;
-    btScalar *ll = L + nskip;
-    for (int p=1; p<n; ll+=nskip, ++p) {
-      btScalar Wp = W1[p];
-      btScalar ell = *ll;
-      W1[p] =    Wp - W11*ell;
-      W2[p] = k1*Wp +  k2*ell;
-    }
-  }
+	btScalar alpha1 = btScalar(1.0);
+	btScalar alpha2 = btScalar(1.0);
 
-  btScalar *ll = L + (nskip + 1);
-  for (int j=1; j<n; ll+=nskip+1, ++j) {
-    btScalar k1 = W1[j];
-    btScalar k2 = W2[j];
-
-    btScalar dee = d[j];
-    btScalar alphanew = alpha1 + (k1*k1)*dee;
-    btAssert(alphanew != btScalar(0.0));
-    dee /= alphanew;
-    btScalar gamma1 = k1 * dee;
-    dee *= alpha1;
-    alpha1 = alphanew;
-    alphanew = alpha2 - (k2*k2)*dee;
-    dee /= alphanew;
-    btScalar gamma2 = k2 * dee;
-    dee *= alpha2;
-    d[j] = dee;
-    alpha2 = alphanew;
-
-    btScalar *l = ll + nskip;
-    for (int p=j+1; p<n; l+=nskip, ++p) {
-      btScalar ell = *l;
-      btScalar Wp = W1[p] - k1 * ell;
-      ell += gamma1 * Wp;
-      W1[p] = Wp;
-      Wp = W2[p] - k2 * ell;
-      ell -= gamma2 * Wp;
-      W2[p] = Wp;
-      *l = ell;
-    }
-  }
+	{
+		btScalar dee = d[0];
+		btScalar alphanew = alpha1 + (W11 * W11) * dee;
+		btAssert(alphanew != btScalar(0.0));
+		dee /= alphanew;
+		btScalar gamma1 = W11 * dee;
+		dee *= alpha1;
+		alpha1 = alphanew;
+		alphanew = alpha2 - (W21 * W21) * dee;
+		dee /= alphanew;
+		//btScalar gamma2 = W21 * dee;
+		alpha2 = alphanew;
+		btScalar k1 = btScalar(1.0) - W21 * gamma1;
+		btScalar k2 = W21 * gamma1 * W11 - W21;
+		btScalar *ll = L + nskip;
+		for (int p = 1; p < n; ll += nskip, ++p)
+		{
+			btScalar Wp = W1[p];
+			btScalar ell = *ll;
+			W1[p] = Wp - W11 * ell;
+			W2[p] = k1 * Wp + k2 * ell;
+		}
+	}
+
+	btScalar *ll = L + (nskip + 1);
+	for (int j = 1; j < n; ll += nskip + 1, ++j)
+	{
+		btScalar k1 = W1[j];
+		btScalar k2 = W2[j];
+
+		btScalar dee = d[j];
+		btScalar alphanew = alpha1 + (k1 * k1) * dee;
+		btAssert(alphanew != btScalar(0.0));
+		dee /= alphanew;
+		btScalar gamma1 = k1 * dee;
+		dee *= alpha1;
+		alpha1 = alphanew;
+		alphanew = alpha2 - (k2 * k2) * dee;
+		dee /= alphanew;
+		btScalar gamma2 = k2 * dee;
+		dee *= alpha2;
+		d[j] = dee;
+		alpha2 = alphanew;
+
+		btScalar *l = ll + nskip;
+		for (int p = j + 1; p < n; l += nskip, ++p)
+		{
+			btScalar ell = *l;
+			btScalar Wp = W1[p] - k1 * ell;
+			ell += gamma1 * Wp;
+			W1[p] = Wp;
+			Wp = W2[p] - k2 * ell;
+			ell -= gamma2 * Wp;
+			W2[p] = Wp;
+			*l = ell;
+		}
+	}
 }
 
-
-#define _BTGETA(i,j) (A[i][j])
+#define _BTGETA(i, j) (A[i][j])
 //#define _GETA(i,j) (A[(i)*nskip+(j)])
-#define BTGETA(i,j) ((i > j) ? _BTGETA(i,j) : _BTGETA(j,i))
+#define BTGETA(i, j) ((i > j) ? _BTGETA(i, j) : _BTGETA(j, i))
 
 inline size_t btEstimateLDLTAddTLTmpbufSize(int nskip)
 {
-  return nskip * 2 * sizeof(btScalar);
+	return nskip * 2 * sizeof(btScalar);
 }
 
-
-void btLDLTRemove (btScalar **A, const int *p, btScalar *L, btScalar *d,
-    int n1, int n2, int r, int nskip, btAlignedObjectArray<btScalar>& scratch)
+void btLDLTRemove(btScalar **A, const int *p, btScalar *L, btScalar *d,
+				  int n1, int n2, int r, int nskip, btAlignedObjectArray<btScalar> &scratch)
 {
-  btAssert(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 &&
-	   n1 >= n2 && nskip >= n1);
-  #ifdef BT_DEBUG
-	for (int i=0; i<n2; ++i) 
+	btAssert(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 &&
+			 n1 >= n2 && nskip >= n1);
+#ifdef BT_DEBUG
+	for (int i = 0; i < n2; ++i)
 		btAssert(p[i] >= 0 && p[i] < n1);
-  #endif
-
-  if (r==n2-1) {
-    return;		// deleting last row/col is easy
-  }
-  else {
-    size_t LDLTAddTL_size = btEstimateLDLTAddTLTmpbufSize(nskip);
-    btAssert(LDLTAddTL_size % sizeof(btScalar) == 0);
-	scratch.resize(nskip * 2+n2);
-    btScalar *tmp = &scratch[0];
-    if (r==0) {
-      btScalar *a = (btScalar *)((char *)tmp + LDLTAddTL_size);
-      const int p_0 = p[0];
-      for (int i=0; i<n2; ++i) {
-        a[i] = -BTGETA(p[i],p_0);
-      }
-      a[0] += btScalar(1.0);
-      btLDLTAddTL (L,d,a,n2,nskip,scratch);
-    }
-    else {
-      btScalar *t = (btScalar *)((char *)tmp + LDLTAddTL_size);
-      {
-        btScalar *Lcurr = L + r*nskip;
-        for (int i=0; i<r; ++Lcurr, ++i) {
-          btAssert(d[i] != btScalar(0.0));
-          t[i] = *Lcurr / d[i];
-        }
-      }
-      btScalar *a = t + r;
-      {
-        btScalar *Lcurr = L + r*nskip;
-        const int *pp_r = p + r, p_r = *pp_r;
-        const int n2_minus_r = n2-r;
-        for (int i=0; i<n2_minus_r; Lcurr+=nskip,++i) {
-          a[i] = btLargeDot(Lcurr,t,r) - BTGETA(pp_r[i],p_r);
-        }
-      }
-      a[0] += btScalar(1.0);
-      btLDLTAddTL (L + r*nskip+r, d+r, a, n2-r, nskip, scratch);
-    }
-  }
+#endif
 
-  // snip out row/column r from L and d
-  btRemoveRowCol (L,n2,nskip,r);
-  if (r < (n2-1)) memmove (d+r,d+r+1,(n2-r-1)*sizeof(btScalar));
+	if (r == n2 - 1)
+	{
+		return;  // deleting last row/col is easy
+	}
+	else
+	{
+		size_t LDLTAddTL_size = btEstimateLDLTAddTLTmpbufSize(nskip);
+		btAssert(LDLTAddTL_size % sizeof(btScalar) == 0);
+		scratch.resize(nskip * 2 + n2);
+		btScalar *tmp = &scratch[0];
+		if (r == 0)
+		{
+			btScalar *a = (btScalar *)((char *)tmp + LDLTAddTL_size);
+			const int p_0 = p[0];
+			for (int i = 0; i < n2; ++i)
+			{
+				a[i] = -BTGETA(p[i], p_0);
+			}
+			a[0] += btScalar(1.0);
+			btLDLTAddTL(L, d, a, n2, nskip, scratch);
+		}
+		else
+		{
+			btScalar *t = (btScalar *)((char *)tmp + LDLTAddTL_size);
+			{
+				btScalar *Lcurr = L + r * nskip;
+				for (int i = 0; i < r; ++Lcurr, ++i)
+				{
+					btAssert(d[i] != btScalar(0.0));
+					t[i] = *Lcurr / d[i];
+				}
+			}
+			btScalar *a = t + r;
+			{
+				btScalar *Lcurr = L + r * nskip;
+				const int *pp_r = p + r, p_r = *pp_r;
+				const int n2_minus_r = n2 - r;
+				for (int i = 0; i < n2_minus_r; Lcurr += nskip, ++i)
+				{
+					a[i] = btLargeDot(Lcurr, t, r) - BTGETA(pp_r[i], p_r);
+				}
+			}
+			a[0] += btScalar(1.0);
+			btLDLTAddTL(L + r * nskip + r, d + r, a, n2 - r, nskip, scratch);
+		}
+	}
+
+	// snip out row/column r from L and d
+	btRemoveRowCol(L, n2, nskip, r);
+	if (r < (n2 - 1)) memmove(d + r, d + r + 1, (n2 - r - 1) * sizeof(btScalar));
 }
 
-
-void btLCP::transfer_i_from_C_to_N (int i, btAlignedObjectArray<btScalar>& scratch)
+void btLCP::transfer_i_from_C_to_N(int i, btAlignedObjectArray<btScalar> &scratch)
 {
-  {
-    int *C = m_C;
-    // remove a row/column from the factorization, and adjust the
-    // indexes (black magic!)
-    int last_idx = -1;
-    const int nC = m_nC;
-    int j = 0;
-    for ( ; j<nC; ++j) {
-      if (C[j]==nC-1) {
-        last_idx = j;
-      }
-      if (C[j]==i) {
-        btLDLTRemove (m_A,C,m_L,m_d,m_n,nC,j,m_nskip,scratch);
-        int k;
-        if (last_idx == -1) {
-          for (k=j+1 ; k<nC; ++k) {
-            if (C[k]==nC-1) {
-              break;
-            }
-          }
-          btAssert (k < nC);
-        }
-        else {
-          k = last_idx;
-        }
-        C[k] = C[j];
-        if (j < (nC-1)) memmove (C+j,C+j+1,(nC-j-1)*sizeof(int));
-        break;
-      }
-    }
-    btAssert (j < nC);
-
-    btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,i,nC-1,m_nskip,1);
-
-    m_nN++;
-    m_nC = nC - 1; // nC value is outdated after this line
-  }
-
+	{
+		int *C = m_C;
+		// remove a row/column from the factorization, and adjust the
+		// indexes (black magic!)
+		int last_idx = -1;
+		const int nC = m_nC;
+		int j = 0;
+		for (; j < nC; ++j)
+		{
+			if (C[j] == nC - 1)
+			{
+				last_idx = j;
+			}
+			if (C[j] == i)
+			{
+				btLDLTRemove(m_A, C, m_L, m_d, m_n, nC, j, m_nskip, scratch);
+				int k;
+				if (last_idx == -1)
+				{
+					for (k = j + 1; k < nC; ++k)
+					{
+						if (C[k] == nC - 1)
+						{
+							break;
+						}
+					}
+					btAssert(k < nC);
+				}
+				else
+				{
+					k = last_idx;
+				}
+				C[k] = C[j];
+				if (j < (nC - 1)) memmove(C + j, C + j + 1, (nC - j - 1) * sizeof(int));
+				break;
+			}
+		}
+		btAssert(j < nC);
+
+		btSwapProblem(m_A, m_x, m_b, m_w, m_lo, m_hi, m_p, m_state, m_findex, m_n, i, nC - 1, m_nskip, 1);
+
+		m_nN++;
+		m_nC = nC - 1;  // nC value is outdated after this line
+	}
 }
 
-
-void btLCP::pN_equals_ANC_times_qC (btScalar *p, btScalar *q)
+void btLCP::pN_equals_ANC_times_qC(btScalar *p, btScalar *q)
 {
-  // we could try to make this matrix-vector multiplication faster using
-  // outer product matrix tricks, e.g. with the dMultidotX() functions.
-  // but i tried it and it actually made things slower on random 100x100
-  // problems because of the overhead involved. so we'll stick with the
-  // simple method for now.
-  const int nC = m_nC;
-  btScalar *ptgt = p + nC;
-  const int nN = m_nN;
-  for (int i=0; i<nN; ++i) {
-    ptgt[i] = btLargeDot (BTAROW(i+nC),q,nC);
-  }
+	// we could try to make this matrix-vector multiplication faster using
+	// outer product matrix tricks, e.g. with the dMultidotX() functions.
+	// but i tried it and it actually made things slower on random 100x100
+	// problems because of the overhead involved. so we'll stick with the
+	// simple method for now.
+	const int nC = m_nC;
+	btScalar *ptgt = p + nC;
+	const int nN = m_nN;
+	for (int i = 0; i < nN; ++i)
+	{
+		ptgt[i] = btLargeDot(BTAROW(i + nC), q, nC);
+	}
 }
 
-
-void btLCP::pN_plusequals_ANi (btScalar *p, int i, int sign)
+void btLCP::pN_plusequals_ANi(btScalar *p, int i, int sign)
 {
-  const int nC = m_nC;
-  btScalar *aptr = BTAROW(i) + nC;
-  btScalar *ptgt = p + nC;
-  if (sign > 0) {
-    const int nN = m_nN;
-    for (int j=0; j<nN; ++j) ptgt[j] += aptr[j];
-  }
-  else {
-    const int nN = m_nN;
-    for (int j=0; j<nN; ++j) ptgt[j] -= aptr[j];
-  }
+	const int nC = m_nC;
+	btScalar *aptr = BTAROW(i) + nC;
+	btScalar *ptgt = p + nC;
+	if (sign > 0)
+	{
+		const int nN = m_nN;
+		for (int j = 0; j < nN; ++j) ptgt[j] += aptr[j];
+	}
+	else
+	{
+		const int nN = m_nN;
+		for (int j = 0; j < nN; ++j) ptgt[j] -= aptr[j];
+	}
 }
 
-void btLCP::pC_plusequals_s_times_qC (btScalar *p, btScalar s, btScalar *q)
+void btLCP::pC_plusequals_s_times_qC(btScalar *p, btScalar s, btScalar *q)
 {
-  const int nC = m_nC;
-  for (int i=0; i<nC; ++i) {
-    p[i] += s*q[i];
-  }
+	const int nC = m_nC;
+	for (int i = 0; i < nC; ++i)
+	{
+		p[i] += s * q[i];
+	}
 }
 
-void btLCP::pN_plusequals_s_times_qN (btScalar *p, btScalar s, btScalar *q)
+void btLCP::pN_plusequals_s_times_qN(btScalar *p, btScalar s, btScalar *q)
 {
-  const int nC = m_nC;
-  btScalar *ptgt = p + nC, *qsrc = q + nC;
-  const int nN = m_nN;
-  for (int i=0; i<nN; ++i) {
-    ptgt[i] += s*qsrc[i];
-  }
+	const int nC = m_nC;
+	btScalar *ptgt = p + nC, *qsrc = q + nC;
+	const int nN = m_nN;
+	for (int i = 0; i < nN; ++i)
+	{
+		ptgt[i] += s * qsrc[i];
+	}
 }
 
-void btLCP::solve1 (btScalar *a, int i, int dir, int only_transfer)
+void btLCP::solve1(btScalar *a, int i, int dir, int only_transfer)
 {
-  // the `Dell' and `ell' that are computed here are saved. if index i is
-  // later added to the factorization then they can be reused.
-  //
-  // @@@ question: do we need to solve for entire delta_x??? yes, but
-  //     only if an x goes below 0 during the step.
-
-  if (m_nC > 0) {
-    {
-      btScalar *Dell = m_Dell;
-      int *C = m_C;
-      btScalar *aptr = BTAROW(i);
-#   ifdef BTNUB_OPTIMIZATIONS
-      // if nub>0, initial part of aptr[] is guaranteed unpermuted
-      const int nub = m_nub;
-      int j=0;
-      for ( ; j<nub; ++j) Dell[j] = aptr[j];
-      const int nC = m_nC;
-      for ( ; j<nC; ++j) Dell[j] = aptr[C[j]];
-#   else
-      const int nC = m_nC;
-      for (int j=0; j<nC; ++j) Dell[j] = aptr[C[j]];
-#   endif
-    }
-    btSolveL1 (m_L,m_Dell,m_nC,m_nskip);
-    {
-      btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d;
-      const int nC = m_nC;
-      for (int j=0; j<nC; ++j) ell[j] = Dell[j] * d[j];
-    }
+	// the `Dell' and `ell' that are computed here are saved. if index i is
+	// later added to the factorization then they can be reused.
+	//
+	// @@@ question: do we need to solve for entire delta_x??? yes, but
+	//     only if an x goes below 0 during the step.
 
-    if (!only_transfer) {
-      btScalar *tmp = m_tmp, *ell = m_ell;
-      {
-        const int nC = m_nC;
-        for (int j=0; j<nC; ++j) tmp[j] = ell[j];
-      }
-      btSolveL1T (m_L,tmp,m_nC,m_nskip);
-      if (dir > 0) {
-        int *C = m_C;
-        btScalar *tmp = m_tmp;
-        const int nC = m_nC;
-        for (int j=0; j<nC; ++j) a[C[j]] = -tmp[j];
-      } else {
-        int *C = m_C;
-        btScalar *tmp = m_tmp;
-        const int nC = m_nC;
-        for (int j=0; j<nC; ++j) a[C[j]] = tmp[j];
-      }
-    }
-  }
-}
+	if (m_nC > 0)
+	{
+		{
+			btScalar *Dell = m_Dell;
+			int *C = m_C;
+			btScalar *aptr = BTAROW(i);
+#ifdef BTNUB_OPTIMIZATIONS
+			// if nub>0, initial part of aptr[] is guaranteed unpermuted
+			const int nub = m_nub;
+			int j = 0;
+			for (; j < nub; ++j) Dell[j] = aptr[j];
+			const int nC = m_nC;
+			for (; j < nC; ++j) Dell[j] = aptr[C[j]];
+#else
+			const int nC = m_nC;
+			for (int j = 0; j < nC; ++j) Dell[j] = aptr[C[j]];
+#endif
+		}
+		btSolveL1(m_L, m_Dell, m_nC, m_nskip);
+		{
+			btScalar *ell = m_ell, *Dell = m_Dell, *d = m_d;
+			const int nC = m_nC;
+			for (int j = 0; j < nC; ++j) ell[j] = Dell[j] * d[j];
+		}
 
+		if (!only_transfer)
+		{
+			btScalar *tmp = m_tmp, *ell = m_ell;
+			{
+				const int nC = m_nC;
+				for (int j = 0; j < nC; ++j) tmp[j] = ell[j];
+			}
+			btSolveL1T(m_L, tmp, m_nC, m_nskip);
+			if (dir > 0)
+			{
+				int *C = m_C;
+				btScalar *tmp = m_tmp;
+				const int nC = m_nC;
+				for (int j = 0; j < nC; ++j) a[C[j]] = -tmp[j];
+			}
+			else
+			{
+				int *C = m_C;
+				btScalar *tmp = m_tmp;
+				const int nC = m_nC;
+				for (int j = 0; j < nC; ++j) a[C[j]] = tmp[j];
+			}
+		}
+	}
+}
 
 void btLCP::unpermute()
 {
-  // now we have to un-permute x and w
-  {
-    memcpy (m_tmp,m_x,m_n*sizeof(btScalar));
-    btScalar *x = m_x, *tmp = m_tmp;
-    const int *p = m_p;
-    const int n = m_n;
-    for (int j=0; j<n; ++j) x[p[j]] = tmp[j];
-  }
-  {
-    memcpy (m_tmp,m_w,m_n*sizeof(btScalar));
-    btScalar *w = m_w, *tmp = m_tmp;
-    const int *p = m_p;
-    const int n = m_n;
-    for (int j=0; j<n; ++j) w[p[j]] = tmp[j];
-  }
+	// now we have to un-permute x and w
+	{
+		memcpy(m_tmp, m_x, m_n * sizeof(btScalar));
+		btScalar *x = m_x, *tmp = m_tmp;
+		const int *p = m_p;
+		const int n = m_n;
+		for (int j = 0; j < n; ++j) x[p[j]] = tmp[j];
+	}
+	{
+		memcpy(m_tmp, m_w, m_n * sizeof(btScalar));
+		btScalar *w = m_w, *tmp = m_tmp;
+		const int *p = m_p;
+		const int n = m_n;
+		for (int j = 0; j < n; ++j) w[p[j]] = tmp[j];
+	}
 }
 
-#endif // btLCP_FAST
-
+#endif  // btLCP_FAST
 
 //***************************************************************************
 // an optimized Dantzig LCP driver routine for the lo-hi LCP problem.
 
-bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b,
-                btScalar* outer_w, int nub, btScalar *lo, btScalar *hi, int *findex, btDantzigScratchMemory& scratchMem)
+bool btSolveDantzigLCP(int n, btScalar *A, btScalar *x, btScalar *b,
+					   btScalar *outer_w, int nub, btScalar *lo, btScalar *hi, int *findex, btDantzigScratchMemory &scratchMem)
 {
 	s_error = false;
 
-//	printf("btSolveDantzigLCP n=%d\n",n);
-  btAssert (n>0 && A && x && b && lo && hi && nub >= 0 && nub <= n);
-  btAssert(outer_w);
+	//	printf("btSolveDantzigLCP n=%d\n",n);
+	btAssert(n > 0 && A && x && b && lo && hi && nub >= 0 && nub <= n);
+	btAssert(outer_w);
 
 #ifdef BT_DEBUG
-  {
-    // check restrictions on lo and hi
-    for (int k=0; k<n; ++k) 
-		btAssert (lo[k] <= 0 && hi[k] >= 0);
-  }
-# endif
-
-
-  // if all the variables are unbounded then we can just factor, solve,
-  // and return
-  if (nub >= n) 
-  {
-   
-
-    int nskip = (n);
-    btFactorLDLT (A, outer_w, n, nskip);
-    btSolveLDLT (A, outer_w, b, n, nskip);
-    memcpy (x, b, n*sizeof(btScalar));
-
-    return !s_error;
-  }
-
-  const int nskip = (n);
-  scratchMem.L.resize(n*nskip);
-
-  scratchMem.d.resize(n);
-
-  btScalar *w = outer_w;
-  scratchMem.delta_w.resize(n);
-  scratchMem.delta_x.resize(n);
-  scratchMem.Dell.resize(n);
-  scratchMem.ell.resize(n);
-  scratchMem.Arows.resize(n);
-  scratchMem.p.resize(n);
-  scratchMem.C.resize(n);
-
-  // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
-  scratchMem.state.resize(n);
-
-
-  // create LCP object. note that tmp is set to delta_w to save space, this
-  // optimization relies on knowledge of how tmp is used, so be careful!
-  btLCP lcp(n,nskip,nub,A,x,b,w,lo,hi,&scratchMem.L[0],&scratchMem.d[0],&scratchMem.Dell[0],&scratchMem.ell[0],&scratchMem.delta_w[0],&scratchMem.state[0],findex,&scratchMem.p[0],&scratchMem.C[0],&scratchMem.Arows[0]);
-  int adj_nub = lcp.getNub();
-
-  // loop over all indexes adj_nub..n-1. for index i, if x(i),w(i) satisfy the
-  // LCP conditions then i is added to the appropriate index set. otherwise
-  // x(i),w(i) is driven either +ve or -ve to force it to the valid region.
-  // as we drive x(i), x(C) is also adjusted to keep w(C) at zero.
-  // while driving x(i) we maintain the LCP conditions on the other variables
-  // 0..i-1. we do this by watching out for other x(i),w(i) values going
-  // outside the valid region, and then switching them between index sets
-  // when that happens.
-
-  bool hit_first_friction_index = false;
-  for (int i=adj_nub; i<n; ++i) 
-  {
-    s_error = false;
-    // the index i is the driving index and indexes i+1..n-1 are "dont care",
-    // i.e. when we make changes to the system those x's will be zero and we
-    // don't care what happens to those w's. in other words, we only consider
-    // an (i+1)*(i+1) sub-problem of A*x=b+w.
-
-    // if we've hit the first friction index, we have to compute the lo and
-    // hi values based on the values of x already computed. we have been
-    // permuting the indexes, so the values stored in the findex vector are
-    // no longer valid. thus we have to temporarily unpermute the x vector. 
-    // for the purposes of this computation, 0*infinity = 0 ... so if the
-    // contact constraint's normal force is 0, there should be no tangential
-    // force applied.
-
-    if (!hit_first_friction_index && findex && findex[i] >= 0) {
-      // un-permute x into delta_w, which is not being used at the moment
-      for (int j=0; j<n; ++j) scratchMem.delta_w[scratchMem.p[j]] = x[j];
-
-      // set lo and hi values
-      for (int k=i; k<n; ++k) {
-        btScalar wfk = scratchMem.delta_w[findex[k]];
-        if (wfk == 0) {
-          hi[k] = 0;
-          lo[k] = 0;
-        }
-        else {
-          hi[k] = btFabs (hi[k] * wfk);
-          lo[k] = -hi[k];
-        }
-      }
-      hit_first_friction_index = true;
-    }
-
-    // thus far we have not even been computing the w values for indexes
-    // greater than i, so compute w[i] now.
-    w[i] = lcp.AiC_times_qC (i,x) + lcp.AiN_times_qN (i,x) - b[i];
-
-    // if lo=hi=0 (which can happen for tangential friction when normals are
-    // 0) then the index will be assigned to set N with some state. however,
-    // set C's line has zero size, so the index will always remain in set N.
-    // with the "normal" switching logic, if w changed sign then the index
-    // would have to switch to set C and then back to set N with an inverted
-    // state. this is pointless, and also computationally expensive. to
-    // prevent this from happening, we use the rule that indexes with lo=hi=0
-    // will never be checked for set changes. this means that the state for
-    // these indexes may be incorrect, but that doesn't matter.
-
-    // see if x(i),w(i) is in a valid region
-    if (lo[i]==0 && w[i] >= 0) {
-      lcp.transfer_i_to_N (i);
-      scratchMem.state[i] = false;
-    }
-    else if (hi[i]==0 && w[i] <= 0) {
-      lcp.transfer_i_to_N (i);
-      scratchMem.state[i] = true;
-    }
-    else if (w[i]==0) {
-      // this is a degenerate case. by the time we get to this test we know
-      // that lo != 0, which means that lo < 0 as lo is not allowed to be +ve,
-      // and similarly that hi > 0. this means that the line segment
-      // corresponding to set C is at least finite in extent, and we are on it.
-      // NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C()
-      lcp.solve1 (&scratchMem.delta_x[0],i,0,1);
-
-      lcp.transfer_i_to_C (i);
-    }
-    else {
-      // we must push x(i) and w(i)
-      for (;;) {
-        int dir;
-        btScalar dirf;
-        // find direction to push on x(i)
-        if (w[i] <= 0) {
-          dir = 1;
-          dirf = btScalar(1.0);
-        }
-        else {
-          dir = -1;
-          dirf = btScalar(-1.0);
-        }
-
-        // compute: delta_x(C) = -dir*A(C,C)\A(C,i)
-        lcp.solve1 (&scratchMem.delta_x[0],i,dir);
-
-        // note that delta_x[i] = dirf, but we wont bother to set it
-
-        // compute: delta_w = A*delta_x ... note we only care about
-        // delta_w(N) and delta_w(i), the rest is ignored
-        lcp.pN_equals_ANC_times_qC (&scratchMem.delta_w[0],&scratchMem.delta_x[0]);
-        lcp.pN_plusequals_ANi (&scratchMem.delta_w[0],i,dir);
-        scratchMem.delta_w[i] = lcp.AiC_times_qC (i,&scratchMem.delta_x[0]) + lcp.Aii(i)*dirf;
-
-        // find largest step we can take (size=s), either to drive x(i),w(i)
-        // to the valid LCP region or to drive an already-valid variable
-        // outside the valid region.
-
-        int cmd = 1;		// index switching command
-        int si = 0;		// si = index to switch if cmd>3
-        btScalar s = -w[i]/scratchMem.delta_w[i];
-        if (dir > 0) {
-          if (hi[i] < BT_INFINITY) {
-            btScalar s2 = (hi[i]-x[i])*dirf;	// was (hi[i]-x[i])/dirf	// step to x(i)=hi(i)
-            if (s2 < s) {
-              s = s2;
-              cmd = 3;
-            }
-          }
-        }
-        else {
-          if (lo[i] > -BT_INFINITY) {
-            btScalar s2 = (lo[i]-x[i])*dirf;	// was (lo[i]-x[i])/dirf	// step to x(i)=lo(i)
-            if (s2 < s) {
-              s = s2;
-              cmd = 2;
-            }
-          }
-        }
-
-        {
-          const int numN = lcp.numN();
-          for (int k=0; k < numN; ++k) {
-            const int indexN_k = lcp.indexN(k);
-            if (!scratchMem.state[indexN_k] ? scratchMem.delta_w[indexN_k] < 0 : scratchMem.delta_w[indexN_k] > 0) {
-                // don't bother checking if lo=hi=0
-                if (lo[indexN_k] == 0 && hi[indexN_k] == 0) continue;
-                btScalar s2 = -w[indexN_k] / scratchMem.delta_w[indexN_k];
-                if (s2 < s) {
-                  s = s2;
-                  cmd = 4;
-                  si = indexN_k;
-                }
-            }
-          }
-        }
-
-        {
-          const int numC = lcp.numC();
-          for (int k=adj_nub; k < numC; ++k) {
-            const int indexC_k = lcp.indexC(k);
-            if (scratchMem.delta_x[indexC_k] < 0 && lo[indexC_k] > -BT_INFINITY) {
-              btScalar s2 = (lo[indexC_k]-x[indexC_k]) / scratchMem.delta_x[indexC_k];
-              if (s2 < s) {
-                s = s2;
-                cmd = 5;
-                si = indexC_k;
-              }
-            }
-            if (scratchMem.delta_x[indexC_k] > 0 && hi[indexC_k] < BT_INFINITY) {
-              btScalar s2 = (hi[indexC_k]-x[indexC_k]) / scratchMem.delta_x[indexC_k];
-              if (s2 < s) {
-                s = s2;
-                cmd = 6;
-                si = indexC_k;
-              }
-            }
-          }
-        }
-
-        //static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C",
-        //			     "C->NL","C->NH"};
-        //printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i);
-
-        // if s <= 0 then we've got a problem. if we just keep going then
-        // we're going to get stuck in an infinite loop. instead, just cross
-        // our fingers and exit with the current solution.
-        if (s <= btScalar(0.0)) 
-		{
-//          printf("LCP internal error, s <= 0 (s=%.4e)",(double)s);
-          if (i < n) {
-            btSetZero (x+i,n-i);
-            btSetZero (w+i,n-i);
-          }
-          s_error = true;
-          break;
-        }
-
-        // apply x = x + s * delta_x
-        lcp.pC_plusequals_s_times_qC (x, s, &scratchMem.delta_x[0]);
-        x[i] += s * dirf;
-
-        // apply w = w + s * delta_w
-        lcp.pN_plusequals_s_times_qN (w, s, &scratchMem.delta_w[0]);
-        w[i] += s * scratchMem.delta_w[i];
-
-//        void *tmpbuf;
-        // switch indexes between sets if necessary
-        switch (cmd) {
-        case 1:		// done
-          w[i] = 0;
-          lcp.transfer_i_to_C (i);
-          break;
-        case 2:		// done
-          x[i] = lo[i];
-          scratchMem.state[i] = false;
-          lcp.transfer_i_to_N (i);
-          break;
-        case 3:		// done
-          x[i] = hi[i];
-          scratchMem.state[i] = true;
-          lcp.transfer_i_to_N (i);
-          break;
-        case 4:		// keep going
-          w[si] = 0;
-          lcp.transfer_i_from_N_to_C (si);
-          break;
-        case 5:		// keep going
-          x[si] = lo[si];
-          scratchMem.state[si] = false;
-		  lcp.transfer_i_from_C_to_N (si, scratchMem.m_scratch);
-          break;
-        case 6:		// keep going
-          x[si] = hi[si];
-          scratchMem.state[si] = true;
-          lcp.transfer_i_from_C_to_N (si, scratchMem.m_scratch);
-          break;
-        }
-
-        if (cmd <= 3) break;
-      } // for (;;)
-    } // else
-
-    if (s_error) 
 	{
-      break;
-    }
-  } // for (int i=adj_nub; i<n; ++i)
+		// check restrictions on lo and hi
+		for (int k = 0; k < n; ++k)
+			btAssert(lo[k] <= 0 && hi[k] >= 0);
+	}
+#endif
 
-  lcp.unpermute();
+	// if all the variables are unbounded then we can just factor, solve,
+	// and return
+	if (nub >= n)
+	{
+		int nskip = (n);
+		btFactorLDLT(A, outer_w, n, nskip);
+		btSolveLDLT(A, outer_w, b, n, nskip);
+		memcpy(x, b, n * sizeof(btScalar));
+
+		return !s_error;
+	}
+
+	const int nskip = (n);
+	scratchMem.L.resize(n * nskip);
+
+	scratchMem.d.resize(n);
+
+	btScalar *w = outer_w;
+	scratchMem.delta_w.resize(n);
+	scratchMem.delta_x.resize(n);
+	scratchMem.Dell.resize(n);
+	scratchMem.ell.resize(n);
+	scratchMem.Arows.resize(n);
+	scratchMem.p.resize(n);
+	scratchMem.C.resize(n);
+
+	// for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
+	scratchMem.state.resize(n);
+
+	// create LCP object. note that tmp is set to delta_w to save space, this
+	// optimization relies on knowledge of how tmp is used, so be careful!
+	btLCP lcp(n, nskip, nub, A, x, b, w, lo, hi, &scratchMem.L[0], &scratchMem.d[0], &scratchMem.Dell[0], &scratchMem.ell[0], &scratchMem.delta_w[0], &scratchMem.state[0], findex, &scratchMem.p[0], &scratchMem.C[0], &scratchMem.Arows[0]);
+	int adj_nub = lcp.getNub();
+
+	// loop over all indexes adj_nub..n-1. for index i, if x(i),w(i) satisfy the
+	// LCP conditions then i is added to the appropriate index set. otherwise
+	// x(i),w(i) is driven either +ve or -ve to force it to the valid region.
+	// as we drive x(i), x(C) is also adjusted to keep w(C) at zero.
+	// while driving x(i) we maintain the LCP conditions on the other variables
+	// 0..i-1. we do this by watching out for other x(i),w(i) values going
+	// outside the valid region, and then switching them between index sets
+	// when that happens.
+
+	bool hit_first_friction_index = false;
+	for (int i = adj_nub; i < n; ++i)
+	{
+		s_error = false;
+		// the index i is the driving index and indexes i+1..n-1 are "dont care",
+		// i.e. when we make changes to the system those x's will be zero and we
+		// don't care what happens to those w's. in other words, we only consider
+		// an (i+1)*(i+1) sub-problem of A*x=b+w.
+
+		// if we've hit the first friction index, we have to compute the lo and
+		// hi values based on the values of x already computed. we have been
+		// permuting the indexes, so the values stored in the findex vector are
+		// no longer valid. thus we have to temporarily unpermute the x vector.
+		// for the purposes of this computation, 0*infinity = 0 ... so if the
+		// contact constraint's normal force is 0, there should be no tangential
+		// force applied.
+
+		if (!hit_first_friction_index && findex && findex[i] >= 0)
+		{
+			// un-permute x into delta_w, which is not being used at the moment
+			for (int j = 0; j < n; ++j) scratchMem.delta_w[scratchMem.p[j]] = x[j];
+
+			// set lo and hi values
+			for (int k = i; k < n; ++k)
+			{
+				btScalar wfk = scratchMem.delta_w[findex[k]];
+				if (wfk == 0)
+				{
+					hi[k] = 0;
+					lo[k] = 0;
+				}
+				else
+				{
+					hi[k] = btFabs(hi[k] * wfk);
+					lo[k] = -hi[k];
+				}
+			}
+			hit_first_friction_index = true;
+		}
+
+		// thus far we have not even been computing the w values for indexes
+		// greater than i, so compute w[i] now.
+		w[i] = lcp.AiC_times_qC(i, x) + lcp.AiN_times_qN(i, x) - b[i];
+
+		// if lo=hi=0 (which can happen for tangential friction when normals are
+		// 0) then the index will be assigned to set N with some state. however,
+		// set C's line has zero size, so the index will always remain in set N.
+		// with the "normal" switching logic, if w changed sign then the index
+		// would have to switch to set C and then back to set N with an inverted
+		// state. this is pointless, and also computationally expensive. to
+		// prevent this from happening, we use the rule that indexes with lo=hi=0
+		// will never be checked for set changes. this means that the state for
+		// these indexes may be incorrect, but that doesn't matter.
+
+		// see if x(i),w(i) is in a valid region
+		if (lo[i] == 0 && w[i] >= 0)
+		{
+			lcp.transfer_i_to_N(i);
+			scratchMem.state[i] = false;
+		}
+		else if (hi[i] == 0 && w[i] <= 0)
+		{
+			lcp.transfer_i_to_N(i);
+			scratchMem.state[i] = true;
+		}
+		else if (w[i] == 0)
+		{
+			// this is a degenerate case. by the time we get to this test we know
+			// that lo != 0, which means that lo < 0 as lo is not allowed to be +ve,
+			// and similarly that hi > 0. this means that the line segment
+			// corresponding to set C is at least finite in extent, and we are on it.
+			// NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C()
+			lcp.solve1(&scratchMem.delta_x[0], i, 0, 1);
+
+			lcp.transfer_i_to_C(i);
+		}
+		else
+		{
+			// we must push x(i) and w(i)
+			for (;;)
+			{
+				int dir;
+				btScalar dirf;
+				// find direction to push on x(i)
+				if (w[i] <= 0)
+				{
+					dir = 1;
+					dirf = btScalar(1.0);
+				}
+				else
+				{
+					dir = -1;
+					dirf = btScalar(-1.0);
+				}
+
+				// compute: delta_x(C) = -dir*A(C,C)\A(C,i)
+				lcp.solve1(&scratchMem.delta_x[0], i, dir);
+
+				// note that delta_x[i] = dirf, but we wont bother to set it
+
+				// compute: delta_w = A*delta_x ... note we only care about
+				// delta_w(N) and delta_w(i), the rest is ignored
+				lcp.pN_equals_ANC_times_qC(&scratchMem.delta_w[0], &scratchMem.delta_x[0]);
+				lcp.pN_plusequals_ANi(&scratchMem.delta_w[0], i, dir);
+				scratchMem.delta_w[i] = lcp.AiC_times_qC(i, &scratchMem.delta_x[0]) + lcp.Aii(i) * dirf;
+
+				// find largest step we can take (size=s), either to drive x(i),w(i)
+				// to the valid LCP region or to drive an already-valid variable
+				// outside the valid region.
+
+				int cmd = 1;  // index switching command
+				int si = 0;   // si = index to switch if cmd>3
+				btScalar s = -w[i] / scratchMem.delta_w[i];
+				if (dir > 0)
+				{
+					if (hi[i] < BT_INFINITY)
+					{
+						btScalar s2 = (hi[i] - x[i]) * dirf;  // was (hi[i]-x[i])/dirf	// step to x(i)=hi(i)
+						if (s2 < s)
+						{
+							s = s2;
+							cmd = 3;
+						}
+					}
+				}
+				else
+				{
+					if (lo[i] > -BT_INFINITY)
+					{
+						btScalar s2 = (lo[i] - x[i]) * dirf;  // was (lo[i]-x[i])/dirf	// step to x(i)=lo(i)
+						if (s2 < s)
+						{
+							s = s2;
+							cmd = 2;
+						}
+					}
+				}
+
+				{
+					const int numN = lcp.numN();
+					for (int k = 0; k < numN; ++k)
+					{
+						const int indexN_k = lcp.indexN(k);
+						if (!scratchMem.state[indexN_k] ? scratchMem.delta_w[indexN_k] < 0 : scratchMem.delta_w[indexN_k] > 0)
+						{
+							// don't bother checking if lo=hi=0
+							if (lo[indexN_k] == 0 && hi[indexN_k] == 0) continue;
+							btScalar s2 = -w[indexN_k] / scratchMem.delta_w[indexN_k];
+							if (s2 < s)
+							{
+								s = s2;
+								cmd = 4;
+								si = indexN_k;
+							}
+						}
+					}
+				}
+
+				{
+					const int numC = lcp.numC();
+					for (int k = adj_nub; k < numC; ++k)
+					{
+						const int indexC_k = lcp.indexC(k);
+						if (scratchMem.delta_x[indexC_k] < 0 && lo[indexC_k] > -BT_INFINITY)
+						{
+							btScalar s2 = (lo[indexC_k] - x[indexC_k]) / scratchMem.delta_x[indexC_k];
+							if (s2 < s)
+							{
+								s = s2;
+								cmd = 5;
+								si = indexC_k;
+							}
+						}
+						if (scratchMem.delta_x[indexC_k] > 0 && hi[indexC_k] < BT_INFINITY)
+						{
+							btScalar s2 = (hi[indexC_k] - x[indexC_k]) / scratchMem.delta_x[indexC_k];
+							if (s2 < s)
+							{
+								s = s2;
+								cmd = 6;
+								si = indexC_k;
+							}
+						}
+					}
+				}
+
+				//static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C",
+				//			     "C->NL","C->NH"};
+				//printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i);
+
+				// if s <= 0 then we've got a problem. if we just keep going then
+				// we're going to get stuck in an infinite loop. instead, just cross
+				// our fingers and exit with the current solution.
+				if (s <= btScalar(0.0))
+				{
+					//          printf("LCP internal error, s <= 0 (s=%.4e)",(double)s);
+					if (i < n)
+					{
+						btSetZero(x + i, n - i);
+						btSetZero(w + i, n - i);
+					}
+					s_error = true;
+					break;
+				}
+
+				// apply x = x + s * delta_x
+				lcp.pC_plusequals_s_times_qC(x, s, &scratchMem.delta_x[0]);
+				x[i] += s * dirf;
+
+				// apply w = w + s * delta_w
+				lcp.pN_plusequals_s_times_qN(w, s, &scratchMem.delta_w[0]);
+				w[i] += s * scratchMem.delta_w[i];
+
+				//        void *tmpbuf;
+				// switch indexes between sets if necessary
+				switch (cmd)
+				{
+					case 1:  // done
+						w[i] = 0;
+						lcp.transfer_i_to_C(i);
+						break;
+					case 2:  // done
+						x[i] = lo[i];
+						scratchMem.state[i] = false;
+						lcp.transfer_i_to_N(i);
+						break;
+					case 3:  // done
+						x[i] = hi[i];
+						scratchMem.state[i] = true;
+						lcp.transfer_i_to_N(i);
+						break;
+					case 4:  // keep going
+						w[si] = 0;
+						lcp.transfer_i_from_N_to_C(si);
+						break;
+					case 5:  // keep going
+						x[si] = lo[si];
+						scratchMem.state[si] = false;
+						lcp.transfer_i_from_C_to_N(si, scratchMem.m_scratch);
+						break;
+					case 6:  // keep going
+						x[si] = hi[si];
+						scratchMem.state[si] = true;
+						lcp.transfer_i_from_C_to_N(si, scratchMem.m_scratch);
+						break;
+				}
+
+				if (cmd <= 3) break;
+			}  // for (;;)
+		}      // else
+
+		if (s_error)
+		{
+			break;
+		}
+	}  // for (int i=adj_nub; i<n; ++i)
 
+	lcp.unpermute();
 
-  return !s_error;
+	return !s_error;
 }
-
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h
index 903832770ae..8d9b2a13e99 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h
@@ -41,7 +41,6 @@ to be implemented. the first `nub' variables are assumed to have findex < 0.
 
 */
 
-
 #ifndef _BT_LCP_H_
 #define _BT_LCP_H_
 
@@ -49,7 +48,6 @@ to be implemented. the first `nub' variables are assumed to have findex < 0.
 #include <stdio.h>
 #include <assert.h>
 
-
 #include "LinearMath/btScalar.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
@@ -62,16 +60,14 @@ struct btDantzigScratchMemory
 	btAlignedObjectArray<btScalar> delta_x;
 	btAlignedObjectArray<btScalar> Dell;
 	btAlignedObjectArray<btScalar> ell;
-	btAlignedObjectArray<btScalar*> Arows;
+	btAlignedObjectArray<btScalar *> Arows;
 	btAlignedObjectArray<int> p;
 	btAlignedObjectArray<int> C;
 	btAlignedObjectArray<bool> state;
 };
 
 //return false if solving failed
-bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b, btScalar *w,
-	int nub, btScalar *lo, btScalar *hi, int *findex,btDantzigScratchMemory& scratch);
-
-
+bool btSolveDantzigLCP(int n, btScalar *A, btScalar *x, btScalar *b, btScalar *w,
+					   int nub, btScalar *lo, btScalar *hi, int *findex, btDantzigScratchMemory &scratch);
 
-#endif //_BT_LCP_H_
+#endif  //_BT_LCP_H_
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h
index 2a2f2d3d32d..1f669751ce7 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h
@@ -20,30 +20,28 @@ subject to the following restrictions:
 #include "btMLCPSolverInterface.h"
 #include "btDantzigLCP.h"
 
-
 class btDantzigSolver : public btMLCPSolverInterface
 {
 protected:
-
 	btScalar m_acceptableUpperLimitSolution;
 
-	btAlignedObjectArray<char>	m_tempBuffer;
+	btAlignedObjectArray<char> m_tempBuffer;
 
 	btAlignedObjectArray<btScalar> m_A;
 	btAlignedObjectArray<btScalar> m_b;
 	btAlignedObjectArray<btScalar> m_x;
 	btAlignedObjectArray<btScalar> m_lo;
 	btAlignedObjectArray<btScalar> m_hi;
-	btAlignedObjectArray<int>	m_dependencies;
+	btAlignedObjectArray<int> m_dependencies;
 	btDantzigScratchMemory m_scratchMemory;
-public:
 
+public:
 	btDantzigSolver()
-		:m_acceptableUpperLimitSolution(btScalar(1000))
+		: m_acceptableUpperLimitSolution(btScalar(1000))
 	{
 	}
 
-	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
+	virtual bool solveMLCP(const btMatrixXu& A, const btVectorXu& b, btVectorXu& x, const btVectorXu& lo, const btVectorXu& hi, const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
 	{
 		bool result = true;
 		int n = b.rows();
@@ -52,14 +50,12 @@ public:
 			int nub = 0;
 			btAlignedObjectArray<btScalar> ww;
 			ww.resize(n);
-	
 
 			const btScalar* Aptr = A.getBufferPointer();
-			m_A.resize(n*n);
-			for (int i=0;i<n*n;i++)
+			m_A.resize(n * n);
+			for (int i = 0; i < n * n; i++)
 			{
 				m_A[i] = Aptr[i];
-
 			}
 
 			m_b.resize(n);
@@ -67,7 +63,7 @@ public:
 			m_lo.resize(n);
 			m_hi.resize(n);
 			m_dependencies.resize(n);
-			for (int i=0;i<n;i++)
+			for (int i = 0; i < n; i++)
 			{
 				m_lo[i] = lo[i];
 				m_hi[i] = hi[i];
@@ -76,13 +72,12 @@ public:
 				m_dependencies[i] = limitDependency[i];
 			}
 
-
-			result = btSolveDantzigLCP (n,&m_A[0],&m_x[0],&m_b[0],&ww[0],nub,&m_lo[0],&m_hi[0],&m_dependencies[0],m_scratchMemory);
+			result = btSolveDantzigLCP(n, &m_A[0], &m_x[0], &m_b[0], &ww[0], nub, &m_lo[0], &m_hi[0], &m_dependencies[0], m_scratchMemory);
 			if (!result)
 				return result;
 
-//			printf("numAllocas = %d\n",numAllocas);
-			for (int i=0;i<n;i++)
+			//			printf("numAllocas = %d\n",numAllocas);
+			for (int i = 0; i < n; i++)
 			{
 				volatile btScalar xx = m_x[i];
 				if (xx != m_x[i])
@@ -98,15 +93,14 @@ public:
 				}
 			}
 
-			for (int i=0;i<n;i++)
+			for (int i = 0; i < n; i++)
 			{
 				x[i] = m_x[i];
 			}
-			
 		}
 
 		return result;
 	}
 };
 
-#endif //BT_DANTZIG_SOLVER_H
+#endif  //BT_DANTZIG_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp
index 1f4015c7c72..954ffaed759 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp
@@ -19,64 +19,60 @@ subject to the following restrictions:
 //Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
 //STL/std::vector replaced by btAlignedObjectArray
 
-
-
 #include "btLemkeAlgorithm.h"
 
 #undef BT_DEBUG_OSTREAM
 #ifdef BT_DEBUG_OSTREAM
 using namespace std;
-#endif //BT_DEBUG_OSTREAM
+#endif  //BT_DEBUG_OSTREAM
 
 btScalar btMachEps()
 {
-	static bool calculated=false;
+	static bool calculated = false;
 	static btScalar machEps = btScalar(1.);
 	if (!calculated)
 	{
-		do {
+		do
+		{
 			machEps /= btScalar(2.0);
 			// If next epsilon yields 1, then break, because current
 			// epsilon is the machine epsilon.
-		}
-		while ((btScalar)(1.0 + (machEps/btScalar(2.0))) != btScalar(1.0));
-//		printf( "\nCalculated Machine epsilon: %G\n", machEps );
-		calculated=true;
+		} while ((btScalar)(1.0 + (machEps / btScalar(2.0))) != btScalar(1.0));
+		//		printf( "\nCalculated Machine epsilon: %G\n", machEps );
+		calculated = true;
 	}
 	return machEps;
 }
 
-btScalar btEpsRoot() {
-	
+btScalar btEpsRoot()
+{
 	static btScalar epsroot = 0.;
 	static bool alreadyCalculated = false;
-	
-	if (!alreadyCalculated) {
+
+	if (!alreadyCalculated)
+	{
 		epsroot = btSqrt(btMachEps());
 		alreadyCalculated = true;
 	}
 	return epsroot;
 }
 
-	 
-
-  btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
+btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
 {
-  
-    
-    steps = 0;
+	steps = 0;
 
-    int dim = m_q.size();
+	int dim = m_q.size();
 #ifdef BT_DEBUG_OSTREAM
-    if(DEBUGLEVEL >= 1) {
-      cout << "Dimension = " << dim << endl;
-    }
-#endif //BT_DEBUG_OSTREAM
+	if (DEBUGLEVEL >= 1)
+	{
+		cout << "Dimension = " << dim << endl;
+	}
+#endif  //BT_DEBUG_OSTREAM
 
 	btVectorXu solutionVector(2 * dim);
 	solutionVector.setZero();
-	  
-	  //, INIT, 0.);
+
+	//, INIT, 0.);
 
 	btMatrixXu ident(dim, dim);
 	ident.setIdentity();
@@ -85,287 +81,289 @@ btScalar btEpsRoot() {
 #endif
 
 	btMatrixXu mNeg = m_M.negative();
-	  
-    btMatrixXu A(dim, 2 * dim + 2);
+
+	btMatrixXu A(dim, 2 * dim + 2);
 	//
-	A.setSubMatrix(0, 0, dim - 1, dim - 1,ident);
-	A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1,mNeg);
+	A.setSubMatrix(0, 0, dim - 1, dim - 1, ident);
+	A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1, mNeg);
 	A.setSubMatrix(0, 2 * dim, dim - 1, 2 * dim, -1.f);
-	A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1,m_q);
+	A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1, m_q);
 
 #ifdef BT_DEBUG_OSTREAM
 	cout << A << std::endl;
-#endif //BT_DEBUG_OSTREAM
+#endif  //BT_DEBUG_OSTREAM
 
+	//   btVectorXu q_;
+	//   q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1);
 
- //   btVectorXu q_;
- //   q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1);
-
-    btAlignedObjectArray<int> basis;
-    //At first, all w-values are in the basis
-    for (int i = 0; i < dim; i++)
-      basis.push_back(i);
+	btAlignedObjectArray<int> basis;
+	//At first, all w-values are in the basis
+	for (int i = 0; i < dim; i++)
+		basis.push_back(i);
 
 	int pivotRowIndex = -1;
 	btScalar minValue = 1e30f;
 	bool greaterZero = true;
-	for (int i=0;i<dim;i++)
+	for (int i = 0; i < dim; i++)
 	{
-		btScalar v =A(i,2*dim+1);
-		if (v<minValue)
+		btScalar v = A(i, 2 * dim + 1);
+		if (v < minValue)
 		{
-			minValue=v;
+			minValue = v;
 			pivotRowIndex = i;
 		}
-		if (v<0)
+		if (v < 0)
 			greaterZero = false;
 	}
-	
 
-	
-  //  int pivotRowIndex = q_.minIndex();//minIndex(q_);     // first row is that with lowest q-value
-    int z0Row = pivotRowIndex;           // remember the col of z0 for ending algorithm afterwards
-    int pivotColIndex = 2 * dim;         // first col is that of z0
+	//  int pivotRowIndex = q_.minIndex();//minIndex(q_);     // first row is that with lowest q-value
+	int z0Row = pivotRowIndex;    // remember the col of z0 for ending algorithm afterwards
+	int pivotColIndex = 2 * dim;  // first col is that of z0
 
 #ifdef BT_DEBUG_OSTREAM
-    if (DEBUGLEVEL >= 3)
+	if (DEBUGLEVEL >= 3)
 	{
-    //  cout << "A: " << A << endl;
-      cout << "pivotRowIndex " << pivotRowIndex << endl;
-      cout << "pivotColIndex " << pivotColIndex << endl;
-      cout << "Basis: ";
-      for (int i = 0; i < basis.size(); i++)
-        cout << basis[i] << " ";
-      cout << endl;
-    }
-#endif //BT_DEBUG_OSTREAM
+		//  cout << "A: " << A << endl;
+		cout << "pivotRowIndex " << pivotRowIndex << endl;
+		cout << "pivotColIndex " << pivotColIndex << endl;
+		cout << "Basis: ";
+		for (int i = 0; i < basis.size(); i++)
+			cout << basis[i] << " ";
+		cout << endl;
+	}
+#endif  //BT_DEBUG_OSTREAM
 
 	if (!greaterZero)
 	{
+		if (maxloops == 0)
+		{
+			maxloops = 100;
+			//        maxloops = UINT_MAX; //TODO: not a really nice way, problem is: maxloops should be 2^dim (=1<<dim), but this could exceed UINT_MAX and thus the result would be 0 and therefore the lemke algorithm wouldn't start but probably would find a solution within less then UINT_MAX steps. Therefore this constant is used as a upper border right now...
+		}
 
-      if (maxloops == 0) {
-		  maxloops = 100;
-//        maxloops = UINT_MAX; //TODO: not a really nice way, problem is: maxloops should be 2^dim (=1<<dim), but this could exceed UINT_MAX and thus the result would be 0 and therefore the lemke algorithm wouldn't start but probably would find a solution within less then UINT_MAX steps. Therefore this constant is used as a upper border right now...
-      }
-
-      /*start looping*/
-      for(steps = 0; steps < maxloops; steps++) {
-
-        GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
-#ifdef BT_DEBUG_OSTREAM
-        if (DEBUGLEVEL >= 3) {
-        //  cout << "A: " << A << endl;
-          cout << "pivotRowIndex " << pivotRowIndex << endl;
-          cout << "pivotColIndex " << pivotColIndex << endl;
-          cout << "Basis: ";
-          for (int i = 0; i < basis.size(); i++)
-            cout << basis[i] << " ";
-          cout << endl;
-        }
-#endif //BT_DEBUG_OSTREAM
-
-        int pivotColIndexOld = pivotColIndex;
-
-        /*find new column index */
-        if (basis[pivotRowIndex] < dim) //if a w-value left the basis get in the correspondent z-value
-          pivotColIndex = basis[pivotRowIndex] + dim;
-        else
-          //else do it the other way round and get in the corresponding w-value
-          pivotColIndex = basis[pivotRowIndex] - dim;
-
-        /*the column becomes part of the basis*/
-        basis[pivotRowIndex] = pivotColIndexOld;
-
-        pivotRowIndex = findLexicographicMinimum(A, pivotColIndex);
-
-        if(z0Row == pivotRowIndex) { //if z0 leaves the basis the solution is found --> one last elimination step is necessary
-          GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
-          basis[pivotRowIndex] = pivotColIndex; //update basis
-          break;
-      }
-
-      }
+		/*start looping*/
+		for (steps = 0; steps < maxloops; steps++)
+		{
+			GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
 #ifdef BT_DEBUG_OSTREAM
-      if(DEBUGLEVEL >= 1) {
-        cout << "Number of loops: " << steps << endl;
-        cout << "Number of maximal loops: " << maxloops << endl;
-      }
-#endif //BT_DEBUG_OSTREAM
-
-      if(!validBasis(basis)) {
-        info = -1;
+			if (DEBUGLEVEL >= 3)
+			{
+				//  cout << "A: " << A << endl;
+				cout << "pivotRowIndex " << pivotRowIndex << endl;
+				cout << "pivotColIndex " << pivotColIndex << endl;
+				cout << "Basis: ";
+				for (int i = 0; i < basis.size(); i++)
+					cout << basis[i] << " ";
+				cout << endl;
+			}
+#endif  //BT_DEBUG_OSTREAM
+
+			int pivotColIndexOld = pivotColIndex;
+
+			/*find new column index */
+			if (basis[pivotRowIndex] < dim)  //if a w-value left the basis get in the correspondent z-value
+				pivotColIndex = basis[pivotRowIndex] + dim;
+			else
+				//else do it the other way round and get in the corresponding w-value
+				pivotColIndex = basis[pivotRowIndex] - dim;
+
+			/*the column becomes part of the basis*/
+			basis[pivotRowIndex] = pivotColIndexOld;
+
+			pivotRowIndex = findLexicographicMinimum(A, pivotColIndex);
+
+			if (z0Row == pivotRowIndex)
+			{  //if z0 leaves the basis the solution is found --> one last elimination step is necessary
+				GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
+				basis[pivotRowIndex] = pivotColIndex;  //update basis
+				break;
+			}
+		}
 #ifdef BT_DEBUG_OSTREAM
-        if(DEBUGLEVEL >= 1)
-          cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
-#endif //BT_DEBUG_OSTREAM
+		if (DEBUGLEVEL >= 1)
+		{
+			cout << "Number of loops: " << steps << endl;
+			cout << "Number of maximal loops: " << maxloops << endl;
+		}
+#endif  //BT_DEBUG_OSTREAM
 
-        return solutionVector;
-      }
+		if (!validBasis(basis))
+		{
+			info = -1;
+#ifdef BT_DEBUG_OSTREAM
+			if (DEBUGLEVEL >= 1)
+				cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
+#endif  //BT_DEBUG_OSTREAM
 
-    }
+			return solutionVector;
+		}
+	}
 #ifdef BT_DEBUG_OSTREAM
-    if (DEBUGLEVEL >= 2) {
-     // cout << "A: " << A << endl;
-      cout << "pivotRowIndex " << pivotRowIndex << endl;
-      cout << "pivotColIndex " << pivotColIndex << endl;
-    }
-#endif //BT_DEBUG_OSTREAM
-
-    for (int i = 0; i < basis.size(); i++)
+	if (DEBUGLEVEL >= 2)
 	{
-      solutionVector[basis[i]] = A(i,2*dim+1);//q_[i];
+		// cout << "A: " << A << endl;
+		cout << "pivotRowIndex " << pivotRowIndex << endl;
+		cout << "pivotColIndex " << pivotColIndex << endl;
 	}
+#endif  //BT_DEBUG_OSTREAM
 
-    info = 0;
+	for (int i = 0; i < basis.size(); i++)
+	{
+		solutionVector[basis[i]] = A(i, 2 * dim + 1);  //q_[i];
+	}
 
-    return solutionVector;
-  }
+	info = 0;
 
-  int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int & pivotColIndex) {
-	  int RowIndex = 0;
-	  int dim = A.rows();
-	  btAlignedObjectArray<btVectorXu> Rows;
-	  for (int row = 0; row < dim; row++) 
-	  {
+	return solutionVector;
+}
 
-		  btVectorXu vec(dim + 1);
-		  vec.setZero();//, INIT, 0.)
-		  Rows.push_back(vec);
-		  btScalar a = A(row, pivotColIndex);
-		  if (a > 0) {
-			  Rows[row][0] = A(row, 2 * dim + 1) / a;
-			  Rows[row][1] = A(row, 2 * dim) / a;
-			  for (int j = 2; j < dim + 1; j++)
-				  Rows[row][j] = A(row, j - 1) / a;
+int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex)
+{
+	int RowIndex = 0;
+	int dim = A.rows();
+	btAlignedObjectArray<btVectorXu> Rows;
+	for (int row = 0; row < dim; row++)
+	{
+		btVectorXu vec(dim + 1);
+		vec.setZero();  //, INIT, 0.)
+		Rows.push_back(vec);
+		btScalar a = A(row, pivotColIndex);
+		if (a > 0)
+		{
+			Rows[row][0] = A(row, 2 * dim + 1) / a;
+			Rows[row][1] = A(row, 2 * dim) / a;
+			for (int j = 2; j < dim + 1; j++)
+				Rows[row][j] = A(row, j - 1) / a;
 
 #ifdef BT_DEBUG_OSTREAM
-		//		if (DEBUGLEVEL) {
-			//	  cout << "Rows(" << row << ") = " << Rows[row] << endl;
+				//		if (DEBUGLEVEL) {
+				//	  cout << "Rows(" << row << ") = " << Rows[row] << endl;
 				// }
-#endif 
-		  }
-	  }
-
-	  for (int i = 0; i < Rows.size(); i++) 
-	  {
-		  if (Rows[i].nrm2() > 0.) {
-
-			  int j = 0;
-			  for (; j < Rows.size(); j++) 
-			  {
-				  if(i != j)
-				  {
-					  if(Rows[j].nrm2() > 0.)
-					  {
-						  btVectorXu test(dim + 1);
-						  for (int ii=0;ii<dim+1;ii++)
-						  {
-							  test[ii] = Rows[j][ii] - Rows[i][ii];
-						  }
-
-						  //=Rows[j] - Rows[i]
-						  if (! LexicographicPositive(test))
-							  break;
-					  }
-				  }
-			  }
-
-			  if (j == Rows.size()) 
-			  {
-				  RowIndex += i;
-				  break;
-			  }
-		  }
-	  }
-
-	  return RowIndex;
-  }
-
-  bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu & v)
-{
-    int i = 0;
-  //  if (DEBUGLEVEL)
-    //  cout << "v " << v << endl;
+#endif
+		}
+	}
 
-    while(i < v.size()-1 && fabs(v[i]) < btMachEps())
-      i++;
-    if (v[i] > 0)
-      return true;
+	for (int i = 0; i < Rows.size(); i++)
+	{
+		if (Rows[i].nrm2() > 0.)
+		{
+			int j = 0;
+			for (; j < Rows.size(); j++)
+			{
+				if (i != j)
+				{
+					if (Rows[j].nrm2() > 0.)
+					{
+						btVectorXu test(dim + 1);
+						for (int ii = 0; ii < dim + 1; ii++)
+						{
+							test[ii] = Rows[j][ii] - Rows[i][ii];
+						}
+
+						//=Rows[j] - Rows[i]
+						if (!LexicographicPositive(test))
+							break;
+					}
+				}
+			}
+
+			if (j == Rows.size())
+			{
+				RowIndex += i;
+				break;
+			}
+		}
+	}
 
-    return false;
-  }
+	return RowIndex;
+}
 
-void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis) 
+bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu& v)
 {
+	int i = 0;
+	//  if (DEBUGLEVEL)
+	//  cout << "v " << v << endl;
+
+	while (i < v.size() - 1 && fabs(v[i]) < btMachEps())
+		i++;
+	if (v[i] > 0)
+		return true;
 
+	return false;
+}
+
+void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis)
+{
 	btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex);
 #ifdef BT_DEBUG_OSTREAM
 	cout << A << std::endl;
 #endif
 
-    for (int i = 0; i < A.rows(); i++)
+	for (int i = 0; i < A.rows(); i++)
 	{
-      if (i != pivotRowIndex)
-	  {
-        for (int j = 0; j < A.cols(); j++)
+		if (i != pivotRowIndex)
 		{
-          if (j != pivotColumnIndex)
-		  {
-			  btScalar v = A(i, j);
-			  v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
-            A.setElem(i, j, v);
-		  }
+			for (int j = 0; j < A.cols(); j++)
+			{
+				if (j != pivotColumnIndex)
+				{
+					btScalar v = A(i, j);
+					v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
+					A.setElem(i, j, v);
+				}
+			}
 		}
-	  }
 	}
 
 #ifdef BT_DEBUG_OSTREAM
 	cout << A << std::endl;
-#endif //BT_DEBUG_OSTREAM
-    for (int i = 0; i < A.cols(); i++) 
+#endif  //BT_DEBUG_OSTREAM
+	for (int i = 0; i < A.cols(); i++)
 	{
-      A.mulElem(pivotRowIndex, i,-a);
-    }
+		A.mulElem(pivotRowIndex, i, -a);
+	}
 #ifdef BT_DEBUG_OSTREAM
 	cout << A << std::endl;
-#endif //#ifdef BT_DEBUG_OSTREAM
+#endif  //#ifdef BT_DEBUG_OSTREAM
 
-    for (int i = 0; i < A.rows(); i++)
+	for (int i = 0; i < A.rows(); i++)
 	{
-      if (i != pivotRowIndex)
-	  {
-        A.setElem(i, pivotColumnIndex,0);
-	  }
+		if (i != pivotRowIndex)
+		{
+			A.setElem(i, pivotColumnIndex, 0);
+		}
 	}
 #ifdef BT_DEBUG_OSTREAM
 	cout << A << std::endl;
-#endif //#ifdef BT_DEBUG_OSTREAM
-  }
+#endif  //#ifdef BT_DEBUG_OSTREAM
+}
 
-  bool btLemkeAlgorithm::greaterZero(const btVectorXu & vector)
+bool btLemkeAlgorithm::greaterZero(const btVectorXu& vector)
 {
-    bool isGreater = true;
-    for (int i = 0; i < vector.size(); i++) {
-      if (vector[i] < 0) {
-        isGreater = false;
-        break;
-      }
-    }
-
-    return isGreater;
-  }
-
-  bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray<int>& basis) 
-  {
-    bool isValid = true;
-    for (int i = 0; i < basis.size(); i++) {
-      if (basis[i] >= basis.size() * 2) { //then z0 is in the base
-        isValid = false;
-        break;
-      }
-    }
-
-    return isValid;
-  }
+	bool isGreater = true;
+	for (int i = 0; i < vector.size(); i++)
+	{
+		if (vector[i] < 0)
+		{
+			isGreater = false;
+			break;
+		}
+	}
 
+	return isGreater;
+}
 
+bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray<int>& basis)
+{
+	bool isValid = true;
+	for (int i = 0; i < basis.size(); i++)
+	{
+		if (basis[i] >= basis.size() * 2)
+		{  //then z0 is in the base
+			isValid = false;
+			break;
+		}
+	}
+
+	return isValid;
+}
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
index 7555cd9d207..3c6bf72a233 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
@@ -19,90 +19,84 @@ subject to the following restrictions:
 //Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
 //STL/std::vector replaced by btAlignedObjectArray
 
-
-
 #ifndef BT_NUMERICS_LEMKE_ALGORITHM_H_
 #define BT_NUMERICS_LEMKE_ALGORITHM_H_
 
 #include "LinearMath/btMatrixX.h"
 
-
-#include <vector> //todo: replace by btAlignedObjectArray
+#include <vector>  //todo: replace by btAlignedObjectArray
 
 class btLemkeAlgorithm
 {
 public:
- 
-
-  btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int & DEBUGLEVEL_ = 0) :
-	  DEBUGLEVEL(DEBUGLEVEL_)
-  {
-	setSystem(M_, q_);
-  }
+	btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int& DEBUGLEVEL_ = 0) : DEBUGLEVEL(DEBUGLEVEL_)
+	{
+		setSystem(M_, q_);
+	}
 
-  /* GETTER / SETTER */
-  /**
+	/* GETTER / SETTER */
+	/**
    * \brief return info of solution process
    */
-  int getInfo() {
-	return info;
-  }
+	int getInfo()
+	{
+		return info;
+	}
 
-  /**
+	/**
    * \brief get the number of steps until the solution was found
    */
-  int getSteps(void) {
-	return steps;
-  }
-
-
+	int getSteps(void)
+	{
+		return steps;
+	}
 
-  /**
+	/**
    * \brief set system with Matrix M and vector q
    */
-  void setSystem(const btMatrixXu & M_, const btVectorXu & q_)
+	void setSystem(const btMatrixXu& M_, const btVectorXu& q_)
 	{
 		m_M = M_;
 		m_q = q_;
-  }
-  /***************************************************/
+	}
+	/***************************************************/
 
-  /**
+	/**
    * \brief solve algorithm adapted from : Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd)
    */
-  btVectorXu solve(unsigned int maxloops = 0);
+	btVectorXu solve(unsigned int maxloops = 0);
 
-  virtual ~btLemkeAlgorithm() {
-  }
+	virtual ~btLemkeAlgorithm()
+	{
+	}
 
 protected:
-  int findLexicographicMinimum(const btMatrixXu &A, const int & pivotColIndex);
-  bool LexicographicPositive(const btVectorXu & v);
-  void GaussJordanEliminationStep(btMatrixXu &A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis);
-  bool greaterZero(const btVectorXu & vector);
-  bool validBasis(const btAlignedObjectArray<int>& basis);
+	int findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex);
+	bool LexicographicPositive(const btVectorXu& v);
+	void GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis);
+	bool greaterZero(const btVectorXu& vector);
+	bool validBasis(const btAlignedObjectArray<int>& basis);
 
-  btMatrixXu m_M;
-  btVectorXu m_q;
+	btMatrixXu m_M;
+	btVectorXu m_q;
 
-  /**
+	/**
    * \brief number of steps until the Lemke algorithm found a solution
    */
-  unsigned int steps;
+	unsigned int steps;
 
-  /**
+	/**
    * \brief define level of debug output
    */
-  int DEBUGLEVEL;
+	int DEBUGLEVEL;
 
-  /**
+	/**
    * \brief did the algorithm find a solution
    *
    * -1 : not successful
    *  0 : successful
    */
-  int info;
+	int info;
 };
 
-
 #endif /* BT_NUMERICS_LEMKE_ALGORITHM_H_ */
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
index 98484c37964..f18c4ea41b0 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
@@ -17,334 +17,322 @@ subject to the following restrictions:
 #ifndef BT_LEMKE_SOLVER_H
 #define BT_LEMKE_SOLVER_H
 
-
 #include "btMLCPSolverInterface.h"
 #include "btLemkeAlgorithm.h"
 
-
-
-
-///The btLemkeSolver is based on "Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) "
+///The btLemkeSolver is based on "Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) "
 ///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time.
 ///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team
 class btLemkeSolver : public btMLCPSolverInterface
 {
 protected:
-	
 public:
-
-	btScalar	m_maxValue;
-	int			m_debugLevel;
-	int			m_maxLoops;
-	bool		m_useLoHighBounds;
-	
-	
+	btScalar m_maxValue;
+	int m_debugLevel;
+	int m_maxLoops;
+	bool m_useLoHighBounds;
 
 	btLemkeSolver()
-		:m_maxValue(100000),
-		m_debugLevel(0),
-		m_maxLoops(1000),
-		m_useLoHighBounds(true)
+		: m_maxValue(100000),
+		  m_debugLevel(0),
+		  m_maxLoops(1000),
+		  m_useLoHighBounds(true)
 	{
 	}
-	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
+	virtual bool solveMLCP(const btMatrixXu& A, const btVectorXu& b, btVectorXu& x, const btVectorXu& lo, const btVectorXu& hi, const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
 	{
-		
 		if (m_useLoHighBounds)
 		{
+			BT_PROFILE("btLemkeSolver::solveMLCP");
+			int n = A.rows();
+			if (0 == n)
+				return true;
 
-		BT_PROFILE("btLemkeSolver::solveMLCP");
-		int n = A.rows();
-		if (0==n)
-			return true;
-		
-		bool fail = false;
-
-		btVectorXu solution(n);
-		btVectorXu q1;
-		q1.resize(n);
-		for (int row=0;row<n;row++)
-		{
-			q1[row] = -b[row];
-		}
+			bool fail = false;
+
+			btVectorXu solution(n);
+			btVectorXu q1;
+			q1.resize(n);
+			for (int row = 0; row < n; row++)
+			{
+				q1[row] = -b[row];
+			}
 
-	//		cout << "A" << endl;
-	//		cout << A << endl;
+			//		cout << "A" << endl;
+			//		cout << A << endl;
 
 			/////////////////////////////////////
 
 			//slow matrix inversion, replace with LU decomposition
 			btMatrixXu A1;
-			btMatrixXu B(n,n);
+			btMatrixXu B(n, n);
 			{
-				BT_PROFILE("inverse(slow)");
-				A1.resize(A.rows(),A.cols());
-				for (int row=0;row<A.rows();row++)
+				//BT_PROFILE("inverse(slow)");
+				A1.resize(A.rows(), A.cols());
+				for (int row = 0; row < A.rows(); row++)
 				{
-					for (int col=0;col<A.cols();col++)
+					for (int col = 0; col < A.cols(); col++)
 					{
-						A1.setElem(row,col,A(row,col));
+						A1.setElem(row, col, A(row, col));
 					}
 				}
 
 				btMatrixXu matrix;
-				matrix.resize(n,2*n);
-				for (int row=0;row<n;row++)
+				matrix.resize(n, 2 * n);
+				for (int row = 0; row < n; row++)
 				{
-					for (int col=0;col<n;col++)
+					for (int col = 0; col < n; col++)
 					{
-						matrix.setElem(row,col,A1(row,col));
+						matrix.setElem(row, col, A1(row, col));
 					}
 				}
 
-
-				btScalar ratio,a;
-				int i,j,k;
-				for(i = 0; i < n; i++){
-				for(j = n; j < 2*n; j++){
-					if(i==(j-n))
-						matrix.setElem(i,j,1.0);
-					else
-						matrix.setElem(i,j,0.0);
+				btScalar ratio, a;
+				int i, j, k;
+				for (i = 0; i < n; i++)
+				{
+					for (j = n; j < 2 * n; j++)
+					{
+						if (i == (j - n))
+							matrix.setElem(i, j, 1.0);
+						else
+							matrix.setElem(i, j, 0.0);
+					}
 				}
-			}
-			for(i = 0; i < n; i++){
-				for(j = 0; j < n; j++){
-					if(i!=j)
+				for (i = 0; i < n; i++)
+				{
+					for (j = 0; j < n; j++)
 					{
-						btScalar v = matrix(i,i);
-						if (btFuzzyZero(v))
+						if (i != j)
 						{
-							a = 0.000001f;
-						}
-						ratio = matrix(j,i)/matrix(i,i);
-						for(k = 0; k < 2*n; k++){
-							matrix.addElem(j,k,- ratio * matrix(i,k));
+							btScalar v = matrix(i, i);
+							if (btFuzzyZero(v))
+							{
+								a = 0.000001f;
+							}
+							ratio = matrix(j, i) / matrix(i, i);
+							for (k = 0; k < 2 * n; k++)
+							{
+								matrix.addElem(j, k, -ratio * matrix(i, k));
+							}
 						}
 					}
 				}
-			}
-			for(i = 0; i < n; i++){
-				a = matrix(i,i);
-				if (btFuzzyZero(a))
+				for (i = 0; i < n; i++)
 				{
-					a = 0.000001f;
-				}
-				btScalar invA = 1.f/a;
-				for(j = 0; j < 2*n; j++){
-					matrix.mulElem(i,j,invA);
+					a = matrix(i, i);
+					if (btFuzzyZero(a))
+					{
+						a = 0.000001f;
+					}
+					btScalar invA = 1.f / a;
+					for (j = 0; j < 2 * n; j++)
+					{
+						matrix.mulElem(i, j, invA);
+					}
 				}
-			}
 
-	
-
-	
-
-			for (int row=0;row<n;row++)
+				for (int row = 0; row < n; row++)
 				{
-					for (int col=0;col<n;col++)
+					for (int col = 0; col < n; col++)
 					{
-						B.setElem(row,col,matrix(row,n+col));
+						B.setElem(row, col, matrix(row, n + col));
 					}
 				}
 			}
 
-		btMatrixXu b1(n,1);
+			btMatrixXu b1(n, 1);
 
-		btMatrixXu M(n*2,n*2);
-		for (int row=0;row<n;row++)
-		{
-			b1.setElem(row,0,-b[row]);
-			for (int col=0;col<n;col++)
+			btMatrixXu M(n * 2, n * 2);
+			for (int row = 0; row < n; row++)
 			{
-				btScalar v =B(row,col);
-				M.setElem(row,col,v);
-				M.setElem(n+row,n+col,v);
-				M.setElem(n+row,col,-v);
-				M.setElem(row,n+col,-v);
-
+				b1.setElem(row, 0, -b[row]);
+				for (int col = 0; col < n; col++)
+				{
+					btScalar v = B(row, col);
+					M.setElem(row, col, v);
+					M.setElem(n + row, n + col, v);
+					M.setElem(n + row, col, -v);
+					M.setElem(row, n + col, -v);
+				}
 			}
-		}
 
-		btMatrixXu Bb1 = B*b1;
-//		q = [ (-B*b1 - lo)'   (hi + B*b1)' ]'
+			btMatrixXu Bb1 = B * b1;
+			//		q = [ (-B*b1 - lo)'   (hi + B*b1)' ]'
 
-		btVectorXu qq;
-		qq.resize(n*2);
-		for (int row=0;row<n;row++)
-		{
-			qq[row] = -Bb1(row,0)-lo[row];
-			qq[n+row] = Bb1(row,0)+hi[row];
-		}
+			btVectorXu qq;
+			qq.resize(n * 2);
+			for (int row = 0; row < n; row++)
+			{
+				qq[row] = -Bb1(row, 0) - lo[row];
+				qq[n + row] = Bb1(row, 0) + hi[row];
+			}
 
-		btVectorXu z1;
+			btVectorXu z1;
 
-		btMatrixXu y1;
-		y1.resize(n,1);
-		btLemkeAlgorithm lemke(M,qq,m_debugLevel);
-		{
-			BT_PROFILE("lemke.solve");
-			lemke.setSystem(M,qq);
-			z1  = lemke.solve(m_maxLoops);
-		}
-		for (int row=0;row<n;row++)
-		{
-			y1.setElem(row,0,z1[2*n+row]-z1[3*n+row]);
-		}
-		btMatrixXu y1_b1(n,1);
-		for (int i=0;i<n;i++)
-		{
-			y1_b1.setElem(i,0,y1(i,0)-b1(i,0));
-		}
+			btMatrixXu y1;
+			y1.resize(n, 1);
+			btLemkeAlgorithm lemke(M, qq, m_debugLevel);
+			{
+				//BT_PROFILE("lemke.solve");
+				lemke.setSystem(M, qq);
+				z1 = lemke.solve(m_maxLoops);
+			}
+			for (int row = 0; row < n; row++)
+			{
+				y1.setElem(row, 0, z1[2 * n + row] - z1[3 * n + row]);
+			}
+			btMatrixXu y1_b1(n, 1);
+			for (int i = 0; i < n; i++)
+			{
+				y1_b1.setElem(i, 0, y1(i, 0) - b1(i, 0));
+			}
 
-		btMatrixXu x1;
+			btMatrixXu x1;
 
-		x1 = B*(y1_b1);
-			
-		for (int row=0;row<n;row++)
-		{
-			solution[row] = x1(row,0);//n];
-		}
+			x1 = B * (y1_b1);
 
-		int errorIndexMax = -1;
-		int errorIndexMin = -1;
-		float errorValueMax = -1e30;
-		float errorValueMin = 1e30;
-		
-		for (int i=0;i<n;i++)
-		{
-			x[i] = solution[i];
-			volatile btScalar check = x[i];
-			if (x[i] != check)
+			for (int row = 0; row < n; row++)
 			{
-				//printf("Lemke result is #NAN\n");
-				x.setZero();
-				return false;
+				solution[row] = x1(row, 0);  //n];
 			}
-			
-			//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver 
-			//we need to figure out why it happens, and fix it, or detect it properly)
-			if (x[i]>m_maxValue)
+
+			int errorIndexMax = -1;
+			int errorIndexMin = -1;
+			float errorValueMax = -1e30;
+			float errorValueMin = 1e30;
+
+			for (int i = 0; i < n; i++)
 			{
-				if (x[i]> errorValueMax)
+				x[i] = solution[i];
+				volatile btScalar check = x[i];
+				if (x[i] != check)
+				{
+					//printf("Lemke result is #NAN\n");
+					x.setZero();
+					return false;
+				}
+
+				//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
+				//we need to figure out why it happens, and fix it, or detect it properly)
+				if (x[i] > m_maxValue)
+				{
+					if (x[i] > errorValueMax)
+					{
+						fail = true;
+						errorIndexMax = i;
+						errorValueMax = x[i];
+					}
+					////printf("x[i] = %f,",x[i]);
+				}
+				if (x[i] < -m_maxValue)
 				{
-					fail = true;
-					errorIndexMax = i;
-					errorValueMax = x[i];
+					if (x[i] < errorValueMin)
+					{
+						errorIndexMin = i;
+						errorValueMin = x[i];
+						fail = true;
+						//printf("x[i] = %f,",x[i]);
+					}
 				}
-				////printf("x[i] = %f,",x[i]);
 			}
-			if (x[i]<-m_maxValue)
+			if (fail)
 			{
-				if (x[i]<errorValueMin)
+				int m_errorCountTimes = 0;
+				if (errorIndexMin < 0)
+					errorValueMin = 0.f;
+				if (errorIndexMax < 0)
+					errorValueMax = 0.f;
+				m_errorCountTimes++;
+				//	printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
+				for (int i = 0; i < n; i++)
 				{
-					errorIndexMin = i;
-					errorValueMin = x[i];
-					fail = true;
-					//printf("x[i] = %f,",x[i]);
+					x[i] = 0.f;
 				}
 			}
+			return !fail;
 		}
-		if (fail)
+		else
+
 		{
-			int m_errorCountTimes = 0;
-			if (errorIndexMin<0)
-				errorValueMin = 0.f;
-			if (errorIndexMax<0)
-				errorValueMax = 0.f;
-			m_errorCountTimes++;
-		//	printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
-			for (int i=0;i<n;i++)
-			{
-				x[i]=0.f;
-			}
-		}
-		return !fail;
-	} else
-		
-	{
 			int dimension = A.rows();
-		if (0==dimension)
-			return true;
-		
-//		printf("================ solving using Lemke/Newton/Fixpoint\n");
-
-		btVectorXu q;
-		q.resize(dimension);
-		for (int row=0;row<dimension;row++)
-		{
-			q[row] = -b[row];
-		}
-		
-		btLemkeAlgorithm lemke(A,q,m_debugLevel);
-		
-		
-		lemke.setSystem(A,q);
-		
-		btVectorXu solution = lemke.solve(m_maxLoops);
-		
-		//check solution
-		
-		bool fail = false;
-		int errorIndexMax = -1;
-		int errorIndexMin = -1;
-		float errorValueMax = -1e30;
-		float errorValueMin = 1e30;
-		
-		for (int i=0;i<dimension;i++)
-		{
-			x[i] = solution[i+dimension];
-			volatile btScalar check = x[i];
-			if (x[i] != check)
+			if (0 == dimension)
+				return true;
+
+			//		printf("================ solving using Lemke/Newton/Fixpoint\n");
+
+			btVectorXu q;
+			q.resize(dimension);
+			for (int row = 0; row < dimension; row++)
 			{
-				x.setZero();
-				return false;
+				q[row] = -b[row];
 			}
-			
-			//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver 
-			//we need to figure out why it happens, and fix it, or detect it properly)
-			if (x[i]>m_maxValue)
+
+			btLemkeAlgorithm lemke(A, q, m_debugLevel);
+
+			lemke.setSystem(A, q);
+
+			btVectorXu solution = lemke.solve(m_maxLoops);
+
+			//check solution
+
+			bool fail = false;
+			int errorIndexMax = -1;
+			int errorIndexMin = -1;
+			float errorValueMax = -1e30;
+			float errorValueMin = 1e30;
+
+			for (int i = 0; i < dimension; i++)
 			{
-				if (x[i]> errorValueMax)
+				x[i] = solution[i + dimension];
+				volatile btScalar check = x[i];
+				if (x[i] != check)
 				{
-					fail = true;
-					errorIndexMax = i;
-					errorValueMax = x[i];
+					x.setZero();
+					return false;
 				}
-				////printf("x[i] = %f,",x[i]);
-			}
-			if (x[i]<-m_maxValue)
-			{
-				if (x[i]<errorValueMin)
+
+				//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
+				//we need to figure out why it happens, and fix it, or detect it properly)
+				if (x[i] > m_maxValue)
+				{
+					if (x[i] > errorValueMax)
+					{
+						fail = true;
+						errorIndexMax = i;
+						errorValueMax = x[i];
+					}
+					////printf("x[i] = %f,",x[i]);
+				}
+				if (x[i] < -m_maxValue)
 				{
-					errorIndexMin = i;
-					errorValueMin = x[i];
-					fail = true;
-					//printf("x[i] = %f,",x[i]);
+					if (x[i] < errorValueMin)
+					{
+						errorIndexMin = i;
+						errorValueMin = x[i];
+						fail = true;
+						//printf("x[i] = %f,",x[i]);
+					}
 				}
 			}
-		}
-		if (fail)
-		{
-			static int errorCountTimes = 0;
-			if (errorIndexMin<0)
-				errorValueMin = 0.f;
-			if (errorIndexMax<0)
-				errorValueMax = 0.f;
-			printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
-			for (int i=0;i<dimension;i++)
+			if (fail)
 			{
-				x[i]=0.f;
+				static int errorCountTimes = 0;
+				if (errorIndexMin < 0)
+					errorValueMin = 0.f;
+				if (errorIndexMax < 0)
+					errorValueMax = 0.f;
+				printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin, errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
+				for (int i = 0; i < dimension; i++)
+				{
+					x[i] = 0.f;
+				}
 			}
-		}
-
-
-		return !fail;
-	}
-	return true;
 
+			return !fail;
+		}
+		return true;
 	}
-
 };
 
-#endif //BT_LEMKE_SOLVER_H
+#endif  //BT_LEMKE_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
index 6688694a928..5d864f27578 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
@@ -19,11 +19,9 @@ subject to the following restrictions:
 #include "LinearMath/btQuickprof.h"
 #include "btSolveProjectedGaussSeidel.h"
 
-
-btMLCPSolver::btMLCPSolver(	 btMLCPSolverInterface* solver)
-:m_solver(solver),
-m_fallback(0),
-m_cfm(0.000001)//0.0000001
+btMLCPSolver::btMLCPSolver(btMLCPSolverInterface* solver)
+	: m_solver(solver),
+	  m_fallback(0)
 {
 }
 
@@ -34,67 +32,65 @@ btMLCPSolver::~btMLCPSolver()
 bool gUseMatrixMultiply = false;
 bool interleaveContactAndFriction = false;
 
-btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
-	btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies, numBodiesUnUsed, manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+	btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodiesUnUsed, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
 	{
 		BT_PROFILE("gather constraint data");
 
-		int numFrictionPerContact = m_tmpSolverContactConstraintPool.size()==m_tmpSolverContactFrictionConstraintPool.size()? 1 : 2;
-
+		int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2;
 
-	//	int numBodies = m_tmpSolverBodyPool.size();
+		//	int numBodies = m_tmpSolverBodyPool.size();
 		m_allConstraintPtrArray.resize(0);
-		m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size());
-		btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size());
-	//	printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size());
+		m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size());
+		btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size());
+		//	printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size());
 
 		int dindex = 0;
-		for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++)
+		for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); i++)
 		{
 			m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
 			m_limitDependencies[dindex++] = -1;
 		}
- 
+
 		///The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
-		
-		int firstContactConstraintOffset=dindex;
+
+		int firstContactConstraintOffset = dindex;
 
 		if (interleaveContactAndFriction)
 		{
-			for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
+			for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
 			{
 				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
 				m_limitDependencies[dindex++] = -1;
-				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]);
-				int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact));
-				m_limitDependencies[dindex++] = findex +firstContactConstraintOffset;
-				if (numFrictionPerContact==2)
+				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]);
+				int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact));
+				m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
+				if (numFrictionPerContact == 2)
 				{
-					m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
-					m_limitDependencies[dindex++] = findex+firstContactConstraintOffset;
+					m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]);
+					m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
 				}
 			}
-		} else
+		}
+		else
 		{
-			for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
+			for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
 			{
 				m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
 				m_limitDependencies[dindex++] = -1;
 			}
-			for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++)
+			for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++)
 			{
 				m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
-				m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset;
+				m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset;
 			}
-			
 		}
 
-
 		if (!m_allConstraintPtrArray.size())
 		{
-			m_A.resize(0,0);
+			m_A.resize(0, 0);
 			m_b.resize(0);
 			m_x.resize(0);
 			m_lo.resize(0);
@@ -103,7 +99,6 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
 		}
 	}
 
-	
 	if (gUseMatrixMultiply)
 	{
 		BT_PROFILE("createMLCP");
@@ -122,7 +117,7 @@ bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
 {
 	bool result = true;
 
-	if (m_A.rows()==0)
+	if (m_A.rows() == 0)
 		return true;
 
 	//if using split impulse, we solve 2 separate (M)LCPs
@@ -130,28 +125,26 @@ bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
 	{
 		btMatrixXu Acopy = m_A;
 		btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
-//		printf("solve first LCP\n");
-		result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
+		//		printf("solve first LCP\n");
+		result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
 		if (result)
-			result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations );
-
-	} else
+			result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations);
+	}
+	else
 	{
-		result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
+		result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
 	}
 	return result;
 }
 
 struct btJointNode
 {
-	int jointIndex;     // pointer to enclosing dxJoint object
-	int otherBodyIndex;       // *other* body this joint is connected to
-	int nextJointNodeIndex;//-1 for null
+	int jointIndex;          // pointer to enclosing dxJoint object
+	int otherBodyIndex;      // *other* body this joint is connected to
+	int nextJointNodeIndex;  //-1 for null
 	int constraintRowIndex;
 };
 
-
-
 void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 {
 	int numContactRows = interleaveContactAndFriction ? 3 : 1;
@@ -164,36 +157,36 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 		m_bSplit.resize(numConstraintRows);
 		m_b.setZero();
 		m_bSplit.setZero();
-		for (int i=0;i<numConstraintRows ;i++)
+		for (int i = 0; i < numConstraintRows; i++)
 		{
 			btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
 			if (!btFuzzyZero(jacDiag))
 			{
 				btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
 				btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
-				m_b[i]=rhs/jacDiag;
-				m_bSplit[i] = rhsPenetration/jacDiag;
+				m_b[i] = rhs / jacDiag;
+				m_bSplit[i] = rhsPenetration / jacDiag;
 			}
-
 		}
 	}
 
-//	btScalar* w = 0;
-//	int nub = 0;
+	//	btScalar* w = 0;
+	//	int nub = 0;
 
 	m_lo.resize(numConstraintRows);
 	m_hi.resize(numConstraintRows);
- 
+
 	{
 		BT_PROFILE("init lo/ho");
 
-		for (int i=0;i<numConstraintRows;i++)
+		for (int i = 0; i < numConstraintRows; i++)
 		{
-			if (0)//m_limitDependencies[i]>=0)
+			if (0)  //m_limitDependencies[i]>=0)
 			{
 				m_lo[i] = -BT_INFINITY;
 				m_hi[i] = BT_INFINITY;
-			} else
+			}
+			else
 			{
 				m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
 				m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
@@ -202,48 +195,48 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 	}
 
 	//
-	int m=m_allConstraintPtrArray.size();
+	int m = m_allConstraintPtrArray.size();
 
 	int numBodies = m_tmpSolverBodyPool.size();
 	btAlignedObjectArray<int> bodyJointNodeArray;
 	{
 		BT_PROFILE("bodyJointNodeArray.resize");
-		bodyJointNodeArray.resize(numBodies,-1);
+		bodyJointNodeArray.resize(numBodies, -1);
 	}
 	btAlignedObjectArray<btJointNode> jointNodeArray;
 	{
 		BT_PROFILE("jointNodeArray.reserve");
-		jointNodeArray.reserve(2*m_allConstraintPtrArray.size());
+		jointNodeArray.reserve(2 * m_allConstraintPtrArray.size());
 	}
 
-	static btMatrixXu J3;
+	btMatrixXu& J3 = m_scratchJ3;
 	{
 		BT_PROFILE("J3.resize");
-		J3.resize(2*m,8);
+		J3.resize(2 * m, 8);
 	}
-	static btMatrixXu JinvM3;
+	btMatrixXu& JinvM3 = m_scratchJInvM3;
 	{
 		BT_PROFILE("JinvM3.resize/setZero");
 
-		JinvM3.resize(2*m,8);
+		JinvM3.resize(2 * m, 8);
 		JinvM3.setZero();
 		J3.setZero();
 	}
-	int cur=0;
+	int cur = 0;
 	int rowOffset = 0;
-	static btAlignedObjectArray<int> ofs;
+	btAlignedObjectArray<int>& ofs = m_scratchOfs;
 	{
 		BT_PROFILE("ofs resize");
 		ofs.resize(0);
 		ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
-	}				
+	}
 	{
 		BT_PROFILE("Compute J and JinvM");
-		int c=0;
+		int c = 0;
 
 		int numRows = 0;
 
-		for (int i=0;i<m_allConstraintPtrArray.size();i+=numRows,c++)
+		for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
 		{
 			ofs[c] = rowOffset;
 			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
@@ -251,14 +244,14 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 			btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
 			btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 
-			numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ;
+			numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
 			if (orgBodyA)
 			{
 				{
-					int slotA=-1;
+					int slotA = -1;
 					//find free jointNode slot for sbA
-					slotA =jointNodeArray.size();
-					jointNodeArray.expand();//NonInitializing();
+					slotA = jointNodeArray.size();
+					jointNodeArray.expand();  //NonInitializing();
 					int prevSlot = bodyJointNodeArray[sbA];
 					bodyJointNodeArray[sbA] = slotA;
 					jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
@@ -266,35 +259,35 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 					jointNodeArray[slotA].constraintRowIndex = i;
 					jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
 				}
-				for (int row=0;row<numRows;row++,cur++)
+				for (int row = 0; row < numRows; row++, cur++)
 				{
-					btVector3 normalInvMass =				m_allConstraintPtrArray[i+row]->m_contactNormal1 *		orgBodyA->getInvMass();
-					btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal *	orgBodyA->getInvInertiaTensorWorld();
+					btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass();
+					btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld();
 
-					for (int r=0;r<3;r++)
+					for (int r = 0; r < 3; r++)
 					{
-						J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal1[r]);
-						J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]);
-						JinvM3.setElem(cur,r,normalInvMass[r]);
-						JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]);
+						J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]);
+						J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]);
+						JinvM3.setElem(cur, r, normalInvMass[r]);
+						JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]);
 					}
-					J3.setElem(cur,3,0);
-					JinvM3.setElem(cur,3,0);
-					J3.setElem(cur,7,0);
-					JinvM3.setElem(cur,7,0);
+					J3.setElem(cur, 3, 0);
+					JinvM3.setElem(cur, 3, 0);
+					J3.setElem(cur, 7, 0);
+					JinvM3.setElem(cur, 7, 0);
 				}
-			} else
+			}
+			else
 			{
 				cur += numRows;
 			}
 			if (orgBodyB)
 			{
-
 				{
-					int slotB=-1;
+					int slotB = -1;
 					//find free jointNode slot for sbA
-					slotB =jointNodeArray.size();
-					jointNodeArray.expand();//NonInitializing();
+					slotB = jointNodeArray.size();
+					jointNodeArray.expand();  //NonInitializing();
 					int prevSlot = bodyJointNodeArray[sbB];
 					bodyJointNodeArray[sbB] = slotB;
 					jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
@@ -303,78 +296,74 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 					jointNodeArray[slotB].constraintRowIndex = i;
 				}
 
-				for (int row=0;row<numRows;row++,cur++)
+				for (int row = 0; row < numRows; row++, cur++)
 				{
-					btVector3 normalInvMassB = m_allConstraintPtrArray[i+row]->m_contactNormal2*orgBodyB->getInvMass();
-					btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
+					btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass();
+					btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
 
-					for (int r=0;r<3;r++)
+					for (int r = 0; r < 3; r++)
 					{
-						J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal2[r]);
-						J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]);
-						JinvM3.setElem(cur,r,normalInvMassB[r]);
-						JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]);
+						J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]);
+						J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]);
+						JinvM3.setElem(cur, r, normalInvMassB[r]);
+						JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]);
 					}
-					J3.setElem(cur,3,0);
-					JinvM3.setElem(cur,3,0);
-					J3.setElem(cur,7,0);
-					JinvM3.setElem(cur,7,0);
+					J3.setElem(cur, 3, 0);
+					JinvM3.setElem(cur, 3, 0);
+					J3.setElem(cur, 7, 0);
+					JinvM3.setElem(cur, 7, 0);
 				}
 			}
 			else
 			{
 				cur += numRows;
 			}
-			rowOffset+=numRows;
-
+			rowOffset += numRows;
 		}
-		
 	}
 
-
 	//compute JinvM = J*invM.
 	const btScalar* JinvM = JinvM3.getBufferPointer();
 
 	const btScalar* Jptr = J3.getBufferPointer();
 	{
 		BT_PROFILE("m_A.resize");
-		m_A.resize(n,n);
+		m_A.resize(n, n);
 	}
-	
+
 	{
 		BT_PROFILE("m_A.setZero");
 		m_A.setZero();
 	}
-	int c=0;
+	int c = 0;
 	{
 		int numRows = 0;
 		BT_PROFILE("Compute A");
-		for (int i=0;i<m_allConstraintPtrArray.size();i+= numRows,c++)
+		for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
 		{
 			int row__ = ofs[c];
 			int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
 			int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
-		//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
-		//	btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+			//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+			//	btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+
+			numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
 
-			numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ;
-					
-			const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
+			const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
 
 			{
 				int startJointNodeA = bodyJointNodeArray[sbA];
-				while (startJointNodeA>=0)
+				while (startJointNodeA >= 0)
 				{
 					int j0 = jointNodeArray[startJointNodeA].jointIndex;
 					int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
-					if (j0<c)
+					if (j0 < c)
 					{
-								 
 						int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
-						size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8*numRowsOther  : 0;
+						size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0;
 						//printf("%d joint i %d and j0: %d: ",count++,i,j0);
-						m_A.multiplyAdd2_p8r ( JinvMrow, 
-						Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther,  row__,ofs[j0]);
+						m_A.multiplyAdd2_p8r(JinvMrow,
+											 Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
 					}
 					startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
 				}
@@ -382,17 +371,17 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 
 			{
 				int startJointNodeB = bodyJointNodeArray[sbB];
-				while (startJointNodeB>=0)
+				while (startJointNodeB >= 0)
 				{
 					int j1 = jointNodeArray[startJointNodeB].jointIndex;
 					int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
 
-					if (j1<c)
+					if (j1 < c)
 					{
-						int numRowsOther =  cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
-						size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8*numRowsOther  : 0;
-						m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, 
-						Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]);
+						int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
+						size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0;
+						m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows,
+											 Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
 					}
 					startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
 				}
@@ -403,27 +392,25 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 			BT_PROFILE("compute diagonal");
 			// compute diagonal blocks of m_A
 
-			int  row__ = 0;
+			int row__ = 0;
 			int numJointRows = m_allConstraintPtrArray.size();
 
-			int jj=0;
-			for (;row__<numJointRows;)
+			int jj = 0;
+			for (; row__ < numJointRows;)
 			{
-
 				//int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
 				int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
-			//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+				//	btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
 				btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 
+				const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
 
-				const unsigned int infom =  row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
-				
-				const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
-				const btScalar *Jrow = Jptr + 2*8*(size_t)row__;
-				m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__);
-				if (orgBodyB) 
+				const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
+				const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__;
+				m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__);
+				if (orgBodyB)
 				{
-					m_A.multiplyAdd2_p8r (JinvMrow + 8*(size_t)infom, Jrow + 8*(size_t)infom, infom, infom,  row__,row__);
+					m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__);
 				}
 				row__ += infom;
 				jj++;
@@ -434,12 +421,12 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 	if (1)
 	{
 		// add cfm to the diagonal of m_A
-		for ( int i=0; i<m_A.rows(); ++i) 
+		for (int i = 0; i < m_A.rows(); ++i)
 		{
-			m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
+			m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
 		}
 	}
-				   
+
 	///fill the upper triangle of the matrix, to make it symmetric
 	{
 		BT_PROFILE("fill the upper triangle ");
@@ -451,21 +438,21 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 		m_x.resize(numConstraintRows);
 		m_xSplit.resize(numConstraintRows);
 
-		if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING)
+		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 		{
-			for (int i=0;i<m_allConstraintPtrArray.size();i++)
+			for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
 			{
 				const btSolverConstraint& c = *m_allConstraintPtrArray[i];
-				m_x[i]=c.m_appliedImpulse;
+				m_x[i] = c.m_appliedImpulse;
 				m_xSplit[i] = c.m_appliedPushImpulse;
 			}
-		} else
+		}
+		else
 		{
 			m_x.setZero();
 			m_xSplit.setZero();
 		}
 	}
-
 }
 
 void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
@@ -476,120 +463,116 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 	m_b.resize(numConstraintRows);
 	if (infoGlobal.m_splitImpulse)
 		m_bSplit.resize(numConstraintRows);
- 
+
 	m_bSplit.setZero();
 	m_b.setZero();
 
-	for (int i=0;i<numConstraintRows ;i++)
+	for (int i = 0; i < numConstraintRows; i++)
 	{
 		if (m_allConstraintPtrArray[i]->m_jacDiagABInv)
 		{
-			m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv;
+			m_b[i] = m_allConstraintPtrArray[i]->m_rhs / m_allConstraintPtrArray[i]->m_jacDiagABInv;
 			if (infoGlobal.m_splitImpulse)
-				m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv;
+				m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration / m_allConstraintPtrArray[i]->m_jacDiagABInv;
 		}
 	}
- 
-	static btMatrixXu Minv;
-	Minv.resize(6*numBodies,6*numBodies);
+
+	btMatrixXu& Minv = m_scratchMInv;
+	Minv.resize(6 * numBodies, 6 * numBodies);
 	Minv.setZero();
-	for (int i=0;i<numBodies;i++)
+	for (int i = 0; i < numBodies; i++)
 	{
 		const btSolverBody& rb = m_tmpSolverBodyPool[i];
 		const btVector3& invMass = rb.m_invMass;
-		setElem(Minv,i*6+0,i*6+0,invMass[0]);
-		setElem(Minv,i*6+1,i*6+1,invMass[1]);
-		setElem(Minv,i*6+2,i*6+2,invMass[2]);
+		setElem(Minv, i * 6 + 0, i * 6 + 0, invMass[0]);
+		setElem(Minv, i * 6 + 1, i * 6 + 1, invMass[1]);
+		setElem(Minv, i * 6 + 2, i * 6 + 2, invMass[2]);
 		btRigidBody* orgBody = m_tmpSolverBodyPool[i].m_originalBody;
- 
-		for (int r=0;r<3;r++)
-			for (int c=0;c<3;c++)
-				setElem(Minv,i*6+3+r,i*6+3+c,orgBody? orgBody->getInvInertiaTensorWorld()[r][c] : 0);
+
+		for (int r = 0; r < 3; r++)
+			for (int c = 0; c < 3; c++)
+				setElem(Minv, i * 6 + 3 + r, i * 6 + 3 + c, orgBody ? orgBody->getInvInertiaTensorWorld()[r][c] : 0);
 	}
- 
-	static btMatrixXu J;
-	J.resize(numConstraintRows,6*numBodies);
+
+	btMatrixXu& J = m_scratchJ;
+	J.resize(numConstraintRows, 6 * numBodies);
 	J.setZero();
- 
+
 	m_lo.resize(numConstraintRows);
 	m_hi.resize(numConstraintRows);
- 
-	for (int i=0;i<numConstraintRows;i++)
-	{
 
+	for (int i = 0; i < numConstraintRows; i++)
+	{
 		m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
 		m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
- 
+
 		int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA;
 		int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB;
 		if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody)
 		{
-			setElem(J,i,6*bodyIndex0+0,m_allConstraintPtrArray[i]->m_contactNormal1[0]);
-			setElem(J,i,6*bodyIndex0+1,m_allConstraintPtrArray[i]->m_contactNormal1[1]);
-			setElem(J,i,6*bodyIndex0+2,m_allConstraintPtrArray[i]->m_contactNormal1[2]);
-			setElem(J,i,6*bodyIndex0+3,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]);
-			setElem(J,i,6*bodyIndex0+4,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]);
-			setElem(J,i,6*bodyIndex0+5,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]);
+			setElem(J, i, 6 * bodyIndex0 + 0, m_allConstraintPtrArray[i]->m_contactNormal1[0]);
+			setElem(J, i, 6 * bodyIndex0 + 1, m_allConstraintPtrArray[i]->m_contactNormal1[1]);
+			setElem(J, i, 6 * bodyIndex0 + 2, m_allConstraintPtrArray[i]->m_contactNormal1[2]);
+			setElem(J, i, 6 * bodyIndex0 + 3, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]);
+			setElem(J, i, 6 * bodyIndex0 + 4, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]);
+			setElem(J, i, 6 * bodyIndex0 + 5, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]);
 		}
 		if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody)
 		{
-			setElem(J,i,6*bodyIndex1+0,m_allConstraintPtrArray[i]->m_contactNormal2[0]);
-			setElem(J,i,6*bodyIndex1+1,m_allConstraintPtrArray[i]->m_contactNormal2[1]);
-			setElem(J,i,6*bodyIndex1+2,m_allConstraintPtrArray[i]->m_contactNormal2[2]);
-			setElem(J,i,6*bodyIndex1+3,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]);
-			setElem(J,i,6*bodyIndex1+4,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]);
-			setElem(J,i,6*bodyIndex1+5,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]);
+			setElem(J, i, 6 * bodyIndex1 + 0, m_allConstraintPtrArray[i]->m_contactNormal2[0]);
+			setElem(J, i, 6 * bodyIndex1 + 1, m_allConstraintPtrArray[i]->m_contactNormal2[1]);
+			setElem(J, i, 6 * bodyIndex1 + 2, m_allConstraintPtrArray[i]->m_contactNormal2[2]);
+			setElem(J, i, 6 * bodyIndex1 + 3, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]);
+			setElem(J, i, 6 * bodyIndex1 + 4, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]);
+			setElem(J, i, 6 * bodyIndex1 + 5, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]);
 		}
 	}
- 
-	static btMatrixXu J_transpose;
-	J_transpose= J.transpose();
 
-	static btMatrixXu tmp;
+	btMatrixXu& J_transpose = m_scratchJTranspose;
+	J_transpose = J.transpose();
+
+	btMatrixXu& tmp = m_scratchTmp;
 
 	{
 		{
 			BT_PROFILE("J*Minv");
-			tmp = J*Minv;
-
+			tmp = J * Minv;
 		}
 		{
 			BT_PROFILE("J*tmp");
-			m_A = tmp*J_transpose;
+			m_A = tmp * J_transpose;
 		}
 	}
 
 	if (1)
 	{
 		// add cfm to the diagonal of m_A
-		for ( int i=0; i<m_A.rows(); ++i) 
+		for (int i = 0; i < m_A.rows(); ++i)
 		{
-			m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
+			m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
 		}
 	}
 
 	m_x.resize(numConstraintRows);
 	if (infoGlobal.m_splitImpulse)
 		m_xSplit.resize(numConstraintRows);
-//	m_x.setZero();
+	//	m_x.setZero();
 
-	for (int i=0;i<m_allConstraintPtrArray.size();i++)
+	for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
 	{
 		const btSolverConstraint& c = *m_allConstraintPtrArray[i];
-		m_x[i]=c.m_appliedImpulse;
+		m_x[i] = c.m_appliedImpulse;
 		if (infoGlobal.m_splitImpulse)
 			m_xSplit[i] = c.m_appliedPushImpulse;
 	}
-
 }
 
-
-btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	bool result = true;
 	{
 		BT_PROFILE("solveMLCP");
-//		printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols());
+		//		printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols());
 		result = solveMLCP(infoGlobal);
 	}
 
@@ -597,44 +580,41 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
 	if (result)
 	{
 		BT_PROFILE("process MLCP results");
-		for (int i=0;i<m_allConstraintPtrArray.size();i++)
+		for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
 		{
 			{
 				btSolverConstraint& c = *m_allConstraintPtrArray[i];
 				int sbA = c.m_solverBodyIdA;
 				int sbB = c.m_solverBodyIdB;
 				//btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
-			//	btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+				//	btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 
 				btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
 				btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
- 
+
 				{
-					btScalar deltaImpulse = m_x[i]-c.m_appliedImpulse;
+					btScalar deltaImpulse = m_x[i] - c.m_appliedImpulse;
 					c.m_appliedImpulse = m_x[i];
-					solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
-					solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+					solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+					solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
 				}
 
 				if (infoGlobal.m_splitImpulse)
 				{
 					btScalar deltaImpulse = m_xSplit[i] - c.m_appliedPushImpulse;
-					solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
-					solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+					solverBodyA.internalApplyPushImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+					solverBodyB.internalApplyPushImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
 					c.m_appliedPushImpulse = m_xSplit[i];
 				}
-				
 			}
 		}
 	}
 	else
 	{
-	//	printf("m_fallback = %d\n",m_fallback);
+		//	printf("m_fallback = %d\n",m_fallback);
 		m_fallback++;
-		btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+		btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 	}
 
 	return 0.f;
 }
-
-
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
index 43e85445bad..510ae59e58b 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
@@ -23,15 +23,13 @@ subject to the following restrictions:
 
 class btMLCPSolver : public btSequentialImpulseConstraintSolver
 {
-
 protected:
-	
 	btMatrixXu m_A;
 	btVectorXu m_b;
 	btVectorXu m_x;
 	btVectorXu m_lo;
 	btVectorXu m_hi;
-	
+
 	///when using 'split impulse' we solve two separate (M)LCPs
 	btVectorXu m_bSplit;
 	btVectorXu m_xSplit;
@@ -39,14 +37,23 @@ protected:
 	btVectorXu m_xSplit2;
 
 	btAlignedObjectArray<int> m_limitDependencies;
-	btAlignedObjectArray<btSolverConstraint*>	m_allConstraintPtrArray;
+	btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray;
 	btMLCPSolverInterface* m_solver;
 	int m_fallback;
-	btScalar m_cfm;
 
-	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+	/// The following scratch variables are not stateful -- contents are cleared prior to each use.
+	/// They are only cached here to avoid extra memory allocations and deallocations and to ensure
+	/// that multiple instances of the solver can be run in parallel.
+	btMatrixXu m_scratchJ3;
+	btMatrixXu m_scratchJInvM3;
+	btAlignedObjectArray<int> m_scratchOfs;
+	btMatrixXu m_scratchMInv;
+	btMatrixXu m_scratchJ;
+	btMatrixXu m_scratchJTranspose;
+	btMatrixXu m_scratchTmp;
 
+	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 
 	virtual void createMLCP(const btContactSolverInfo& infoGlobal);
 	virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
@@ -55,8 +62,7 @@ protected:
 	virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
 
 public:
-
-	btMLCPSolver(	 btMLCPSolverInterface* solver);
+	btMLCPSolver(btMLCPSolverInterface* solver);
 	virtual ~btMLCPSolver();
 
 	void setMLCPSolver(btMLCPSolverInterface* solver)
@@ -73,21 +79,10 @@ public:
 		m_fallback = num;
 	}
 
-	btScalar	getCfm() const
-	{
-		return m_cfm;
-	}
-	void setCfm(btScalar cfm)
-	{
-		m_cfm = cfm;
-	}
-
-	virtual btConstraintSolverType	getSolverType() const
+	virtual btConstraintSolverType getSolverType() const
 	{
 		return BT_MLCP_SOLVER;
 	}
-
 };
 
-
-#endif //BT_MLCP_SOLVER_H
+#endif  //BT_MLCP_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h
index 25bb3f6d327..6b0465b88d7 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h
@@ -27,7 +27,7 @@ public:
 	}
 
 	//return true is it solves the problem successfully
-	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)=0;
+	virtual bool solveMLCP(const btMatrixXu& A, const btVectorXu& b, btVectorXu& x, const btVectorXu& lo, const btVectorXu& hi, const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true) = 0;
 };
 
-#endif //BT_MLCP_SOLVER_INTERFACE_H
+#endif  //BT_MLCP_SOLVER_INTERFACE_H
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h
index 9ec31a6d4e4..7f8eec3f6e3 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h
@@ -14,38 +14,35 @@ subject to the following restrictions:
 */
 ///original version written by Erwin Coumans, October 2013
 
-
 #ifndef BT_PATH_SOLVER_H
 #define BT_PATH_SOLVER_H
 
 //#define BT_USE_PATH
 #ifdef BT_USE_PATH
 
-extern "C" {
+extern "C"
+{
 #include "PATH/SimpleLCP.h"
 #include "PATH/License.h"
 #include "PATH/Error_Interface.h"
 };
-  void __stdcall MyError(Void *data, Char *msg)
+void __stdcall MyError(Void *data, Char *msg)
 {
-	printf("Path Error: %s\n",msg);
+	printf("Path Error: %s\n", msg);
 }
-  void __stdcall MyWarning(Void *data, Char *msg)
+void __stdcall MyWarning(Void *data, Char *msg)
 {
-	printf("Path Warning: %s\n",msg);
+	printf("Path Warning: %s\n", msg);
 }
 
 Error_Interface e;
 
-
-
 #include "btMLCPSolverInterface.h"
 #include "Dantzig/lcp.h"
 
 class btPathSolver : public btMLCPSolverInterface
 {
 public:
-
 	btPathSolver()
 	{
 		License_SetString("2069810742&Courtesy_License&&&USR&2013&14_12_2011&1000&PATH&GEN&31_12_2013&0_0_0&0&0_0");
@@ -55,17 +52,15 @@ public:
 		Error_SetInterface(&e);
 	}
 
-
-	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
+	virtual bool solveMLCP(const btMatrixXu &A, const btVectorXu &b, btVectorXu &x, const btVectorXu &lo, const btVectorXu &hi, const btAlignedObjectArray<int> &limitDependency, int numIterations, bool useSparsity = true)
 	{
 		MCP_Termination status;
-		
 
 		int numVariables = b.rows();
-		if (0==numVariables)
+		if (0 == numVariables)
 			return true;
 
-			/*	 - variables - the number of variables in the problem
+		/*	 - variables - the number of variables in the problem
 			- m_nnz - the number of nonzeros in the M matrix
 			- m_i - a vector of size m_nnz containing the row indices for M
 			- m_j - a vector of size m_nnz containing the column indices for M
@@ -78,16 +73,16 @@ public:
 		btAlignedObjectArray<int> rowIndices;
 		btAlignedObjectArray<int> colIndices;
 
-		for (int i=0;i<A.rows();i++)
+		for (int i = 0; i < A.rows(); i++)
 		{
-			for (int j=0;j<A.cols();j++)
+			for (int j = 0; j < A.cols(); j++)
 			{
-				if (A(i,j)!=0.f)
+				if (A(i, j) != 0.f)
 				{
 					//add 1, because Path starts at 1, instead of 0
-					rowIndices.push_back(i+1);
-					colIndices.push_back(j+1);
-					values.push_back(A(i,j));
+					rowIndices.push_back(i + 1);
+					colIndices.push_back(j + 1);
+					values.push_back(A(i, j));
 				}
 			}
 		}
@@ -97,19 +92,18 @@ public:
 		btAlignedObjectArray<double> rhs;
 		btAlignedObjectArray<double> upperBounds;
 		btAlignedObjectArray<double> lowerBounds;
-		for (int i=0;i<numVariables;i++)
+		for (int i = 0; i < numVariables; i++)
 		{
 			upperBounds.push_back(hi[i]);
 			lowerBounds.push_back(lo[i]);
 			rhs.push_back(-b[i]);
 		}
 
-
-		SimpleLCP(numVariables,numNonZero,&rowIndices[0],&colIndices[0],&values[0],&rhs[0],&lowerBounds[0],&upperBounds[0], &status, &zResult[0]);
+		SimpleLCP(numVariables, numNonZero, &rowIndices[0], &colIndices[0], &values[0], &rhs[0], &lowerBounds[0], &upperBounds[0], &status, &zResult[0]);
 
 		if (status != MCP_Solved)
 		{
-			static const char* gReturnMsgs[] = {
+			static const char *gReturnMsgs[] = {
 				"Invalid return",
 				"MCP_Solved: The problem was solved",
 				"MCP_NoProgress: A stationary point was found",
@@ -122,16 +116,16 @@ public:
 				"MCP_Infeasible: Problem has no solution",
 				"MCP_Error: An error occurred within the code",
 				"MCP_LicenseError: License could not be found",
-				"MCP_OK"
-			};
+				"MCP_OK"};
 
-			printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);// << std::endl;
+			printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);  // << std::endl;
 			printf("using Projected Gauss Seidel fallback\n");
-			
+
 			return false;
-		} else
+		}
+		else
 		{
-			for (int i=0;i<numVariables;i++)
+			for (int i = 0; i < numVariables; i++)
 			{
 				x[i] = zResult[i];
 				//check for #NAN
@@ -139,13 +133,10 @@ public:
 					return false;
 			}
 			return true;
-
 		}
-
 	}
 };
 
-#endif //BT_USE_PATH
-
+#endif  //BT_USE_PATH
 
-#endif //BT_PATH_SOLVER_H
+#endif  //BT_PATH_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
index 77cc57c6e0e..c3f4ec39974 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
@@ -17,14 +17,22 @@ subject to the following restrictions:
 #ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
 #define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
 
-
 #include "btMLCPSolverInterface.h"
 
 ///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix)
 class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
 {
 public:
-	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
+	btScalar m_leastSquaresResidualThreshold;
+	btScalar m_leastSquaresResidual;
+
+	btSolveProjectedGaussSeidel()
+		: m_leastSquaresResidualThreshold(0),
+		  m_leastSquaresResidual(0)
+	{
+	}
+
+	virtual bool solveMLCP(const btMatrixXu& A, const btVectorXu& b, btVectorXu& x, const btVectorXu& lo, const btVectorXu& hi, const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
 	{
 		if (!A.rows())
 			return true;
@@ -35,52 +43,65 @@ public:
 		btAssert(A.rows() == b.rows());
 
 		int i, j, numRows = A.rows();
-	
-		float delta;
 
-		for (int k = 0; k <numIterations; k++)
+		btScalar delta;
+
+		for (int k = 0; k < numIterations; k++)
 		{
-			for (i = 0; i <numRows; i++)
+			m_leastSquaresResidual = 0.f;
+			for (i = 0; i < numRows; i++)
 			{
 				delta = 0.0f;
 				if (useSparsity)
 				{
-					for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
+					for (int h = 0; h < A.m_rowNonZeroElements1[i].size(); h++)
 					{
-						int j = A.m_rowNonZeroElements1[i][h];
-						if (j != i)//skip main diagonal
+						j = A.m_rowNonZeroElements1[i][h];
+						if (j != i)  //skip main diagonal
 						{
-							delta += A(i,j) * x[j];
+							delta += A(i, j) * x[j];
 						}
 					}
-				} else
+				}
+				else
 				{
-					for (j = 0; j <i; j++) 
-						delta += A(i,j) * x[j];
-					for (j = i+1; j<numRows; j++) 
-						delta += A(i,j) * x[j];
+					for (j = 0; j < i; j++)
+						delta += A(i, j) * x[j];
+					for (j = i + 1; j < numRows; j++)
+						delta += A(i, j) * x[j];
 				}
 
-				float aDiag = A(i,i);
-				x [i] = (b [i] - delta) / aDiag;
-				float s = 1.f;
+				btScalar aDiag = A(i, i);
+				btScalar xOld = x[i];
+				x[i] = (b[i] - delta) / aDiag;
+				btScalar s = 1.f;
 
-				if (limitDependency[i]>=0)
+				if (limitDependency[i] >= 0)
 				{
 					s = x[limitDependency[i]];
-					if (s<0)
-						s=1;
+					if (s < 0)
+						s = 1;
 				}
-			
-				if (x[i]<lo[i]*s)
-					x[i]=lo[i]*s;
-				if (x[i]>hi[i]*s)
-					x[i]=hi[i]*s;
+
+				if (x[i] < lo[i] * s)
+					x[i] = lo[i] * s;
+				if (x[i] > hi[i] * s)
+					x[i] = hi[i] * s;
+				btScalar diff = x[i] - xOld;
+				m_leastSquaresResidual += diff * diff;
+			}
+
+			btScalar eps = m_leastSquaresResidualThreshold;
+			if ((m_leastSquaresResidual < eps) || (k >= (numIterations - 1)))
+			{
+#ifdef VERBOSE_PRINTF_RESIDUAL
+				printf("totalLenSqr = %f at iteration #%d\n", m_leastSquaresResidual, k);
+#endif
+				break;
 			}
 		}
 		return true;
 	}
-
 };
 
-#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
+#endif  //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
index a7b1688469f..fc70d8e637d 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
@@ -24,17 +24,16 @@
 
 #define ROLLING_INFLUENCE_FIX
 
-
 btRigidBody& btActionInterface::getFixedBody()
 {
-	static btRigidBody s_fixed(0, 0,0);
-	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+	static btRigidBody s_fixed(0, 0, 0);
+	s_fixed.setMassProps(btScalar(0.), btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
 	return s_fixed;
 }
 
-btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,	btVehicleRaycaster* raycaster )
-:m_vehicleRaycaster(raycaster),
-m_pitchControl(btScalar(0.))
+btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning, btRigidBody* chassis, btVehicleRaycaster* raycaster)
+	: m_vehicleRaycaster(raycaster),
+	  m_pitchControl(btScalar(0.))
 {
 	m_chassisBody = chassis;
 	m_indexRightAxis = 0;
@@ -43,28 +42,22 @@ m_pitchControl(btScalar(0.))
 	defaultInit(tuning);
 }
 
-
 void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning)
 {
 	(void)tuning;
 	m_currentVehicleSpeedKmHour = btScalar(0.);
 	m_steeringValue = btScalar(0.);
-	
 }
 
-	
-
 btRaycastVehicle::~btRaycastVehicle()
 {
 }
 
-
 //
 // basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed
 //
-btWheelInfo&	btRaycastVehicle::addWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel)
+btWheelInfo& btRaycastVehicle::addWheel(const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0, const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius, const btVehicleTuning& tuning, bool isFrontWheel)
 {
-
 	btWheelInfoConstructionInfo ci;
 
 	ci.m_chassisConnectionCS = connectionPointCS;
@@ -80,76 +73,76 @@ btWheelInfo&	btRaycastVehicle::addWheel( const btVector3& connectionPointCS, con
 	ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm;
 	ci.m_maxSuspensionForce = tuning.m_maxSuspensionForce;
 
-	m_wheelInfo.push_back( btWheelInfo(ci));
-	
-	btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1];
-	
-	updateWheelTransformsWS( wheel , false );
-	updateWheelTransform(getNumWheels()-1,false);
-	return wheel;
-}
-
+	m_wheelInfo.push_back(btWheelInfo(ci));
 
+	btWheelInfo& wheel = m_wheelInfo[getNumWheels() - 1];
 
+	updateWheelTransformsWS(wheel, false);
+	updateWheelTransform(getNumWheels() - 1, false);
+	return wheel;
+}
 
-const btTransform&	btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
+const btTransform& btRaycastVehicle::getWheelTransformWS(int wheelIndex) const
 {
 	btAssert(wheelIndex < getNumWheels());
 	const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
 	return wheel.m_worldTransform;
-
 }
 
-void	btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform)
+void btRaycastVehicle::updateWheelTransform(int wheelIndex, bool interpolatedTransform)
 {
-	
-	btWheelInfo& wheel = m_wheelInfo[ wheelIndex ];
-	updateWheelTransformsWS(wheel,interpolatedTransform);
+	btWheelInfo& wheel = m_wheelInfo[wheelIndex];
+	updateWheelTransformsWS(wheel, interpolatedTransform);
 	btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
 	const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
 	btVector3 fwd = up.cross(right);
 	fwd = fwd.normalize();
-//	up = right.cross(fwd);
-//	up.normalize();
+	//	up = right.cross(fwd);
+	//	up.normalize();
 
 	//rotate around steering over de wheelAxleWS
 	btScalar steering = wheel.m_steering;
-	
-	btQuaternion steeringOrn(up,steering);//wheel.m_steering);
+
+	btQuaternion steeringOrn(up, steering);  //wheel.m_steering);
 	btMatrix3x3 steeringMat(steeringOrn);
 
-	btQuaternion rotatingOrn(right,-wheel.m_rotation);
+	btQuaternion rotatingOrn(right, -wheel.m_rotation);
 	btMatrix3x3 rotatingMat(rotatingOrn);
 
-	btMatrix3x3 basis2(
-		right[0],fwd[0],up[0],
-		right[1],fwd[1],up[1],
-		right[2],fwd[2],up[2]
-	);
-	
+	btMatrix3x3 basis2;
+	basis2[0][m_indexRightAxis] = -right[0];
+	basis2[1][m_indexRightAxis] = -right[1];
+	basis2[2][m_indexRightAxis] = -right[2];
+
+	basis2[0][m_indexUpAxis] = up[0];
+	basis2[1][m_indexUpAxis] = up[1];
+	basis2[2][m_indexUpAxis] = up[2];
+
+	basis2[0][m_indexForwardAxis] = fwd[0];
+	basis2[1][m_indexForwardAxis] = fwd[1];
+	basis2[2][m_indexForwardAxis] = fwd[2];
+
 	wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
 	wheel.m_worldTransform.setOrigin(
-		wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
-	);
+		wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength);
 }
 
 void btRaycastVehicle::resetSuspension()
 {
-
 	int i;
-	for (i=0;i<m_wheelInfo.size();	i++)
+	for (i = 0; i < m_wheelInfo.size(); i++)
 	{
-			btWheelInfo& wheel = m_wheelInfo[i];
-			wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
-			wheel.m_suspensionRelativeVelocity = btScalar(0.0);
-			
-			wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
-			//wheel_info.setContactFriction(btScalar(0.0));
-			wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
+		btWheelInfo& wheel = m_wheelInfo[i];
+		wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
+		wheel.m_suspensionRelativeVelocity = btScalar(0.0);
+
+		wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
+		//wheel_info.setContactFriction(btScalar(0.0));
+		wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
 	}
 }
 
-void	btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform)
+void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel, bool interpolatedTransform)
 {
 	wheel.m_raycastInfo.m_isInContact = false;
 
@@ -159,19 +152,18 @@ void	btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel , bool interpo
 		getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
 	}
 
-	wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS );
-	wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() *  wheel.m_wheelDirectionCS ;
+	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;
 }
 
 btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 {
-	updateWheelTransformsWS( wheel,false);
+	updateWheelTransformsWS(wheel, false);
 
-	
 	btScalar depth = -1;
-	
-	btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius;
+
+	btScalar raylen = wheel.getSuspensionRestLength() + wheel.m_wheelsRadius;
 
 	btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
 	const btVector3& source = wheel.m_raycastInfo.m_hardPointWS;
@@ -179,12 +171,12 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 	const btVector3& target = wheel.m_raycastInfo.m_contactPointWS;
 
 	btScalar param = btScalar(0.);
-	
-	btVehicleRaycaster::btVehicleRaycasterResult	rayResults;
+
+	btVehicleRaycaster::btVehicleRaycasterResult rayResults;
 
 	btAssert(m_vehicleRaycaster);
 
-	void* object = m_vehicleRaycaster->castRay(source,target,rayResults);
+	void* object = m_vehicleRaycaster->castRay(source, target, rayResults);
 
 	wheel.m_raycastInfo.m_groundObject = 0;
 
@@ -192,19 +184,18 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 	{
 		param = rayResults.m_distFraction;
 		depth = raylen * rayResults.m_distFraction;
-		wheel.m_raycastInfo.m_contactNormalWS  = rayResults.m_hitNormalInWorld;
+		wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;
 		wheel.m_raycastInfo.m_isInContact = true;
-		
-		wheel.m_raycastInfo.m_groundObject = &getFixedBody();///@todo for driving on dynamic/movable objects!;
-		//wheel.m_raycastInfo.m_groundObject = object;
 
+		wheel.m_raycastInfo.m_groundObject = &getFixedBody();  ///@todo for driving on dynamic/movable objects!;
+		//wheel.m_raycastInfo.m_groundObject = object;
 
-		btScalar hitDistance = param*raylen;
+		btScalar hitDistance = param * raylen;
 		wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
 		//clamp on max suspension travel
 
-		btScalar  minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01);
-		btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01);
+		btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm * btScalar(0.01);
+		btScalar maxSuspensionLength = wheel.getSuspensionRestLength() + wheel.m_maxSuspensionTravelCm * btScalar(0.01);
 		if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
 		{
 			wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
@@ -216,16 +207,16 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 
 		wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;
 
-		btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS );
+		btScalar denominator = wheel.m_raycastInfo.m_contactNormalWS.dot(wheel.m_raycastInfo.m_wheelDirectionWS);
 
 		btVector3 chassis_velocity_at_contactPoint;
-		btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition();
+		btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
 
 		chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos);
 
-		btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
+		btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint);
 
-		if ( denominator >= btScalar(-0.1))
+		if (denominator >= btScalar(-0.1))
 		{
 			wheel.m_suspensionRelativeVelocity = btScalar(0.0);
 			wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
@@ -236,20 +227,19 @@ btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel)
 			wheel.m_suspensionRelativeVelocity = projVel * inv;
 			wheel.m_clippedInvContactDotSuspension = inv;
 		}
-			
-	} else
+	}
+	else
 	{
 		//put wheel info as in rest position
 		wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
 		wheel.m_suspensionRelativeVelocity = btScalar(0.0);
-		wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS;
+		wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
 		wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
 	}
 
 	return depth;
 }
 
-
 const btTransform& btRaycastVehicle::getChassisWorldTransform() const
 {
 	/*if (getRigidBody()->getMotionState())
@@ -260,26 +250,23 @@ const btTransform& btRaycastVehicle::getChassisWorldTransform() const
 	}
 	*/
 
-	
 	return getRigidBody()->getCenterOfMassTransform();
 }
 
-
-void btRaycastVehicle::updateVehicle( btScalar step )
+void btRaycastVehicle::updateVehicle(btScalar step)
 {
 	{
-		for (int i=0;i<getNumWheels();i++)
+		for (int i = 0; i < getNumWheels(); i++)
 		{
-			updateWheelTransform(i,false);
+			updateWheelTransform(i, false);
 		}
 	}
 
-
 	m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length();
-	
+
 	const btTransform& chassisTrans = getChassisWorldTransform();
 
-	btVector3 forwardW (
+	btVector3 forwardW(
 		chassisTrans.getBasis()[0][m_indexForwardAxis],
 		chassisTrans.getBasis()[1][m_indexForwardAxis],
 		chassisTrans.getBasis()[2][m_indexForwardAxis]);
@@ -292,52 +279,47 @@ void btRaycastVehicle::updateVehicle( btScalar step )
 	//
 	// simulate suspension
 	//
-	
-	int i=0;
-	for (i=0;i<m_wheelInfo.size();i++)
+
+	int i = 0;
+	for (i = 0; i < m_wheelInfo.size(); i++)
 	{
-		//btScalar depth; 
-		//depth = 
-		rayCast( m_wheelInfo[i]);
+		//btScalar depth;
+		//depth =
+		rayCast(m_wheelInfo[i]);
 	}
 
 	updateSuspension(step);
 
-	
-	for (i=0;i<m_wheelInfo.size();i++)
+	for (i = 0; i < m_wheelInfo.size(); i++)
 	{
 		//apply suspension force
 		btWheelInfo& wheel = m_wheelInfo[i];
-		
+
 		btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
-		
+
 		if (suspensionForce > wheel.m_maxSuspensionForce)
 		{
 			suspensionForce = wheel.m_maxSuspensionForce;
 		}
 		btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
 		btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
-		
+
 		getRigidBody()->applyImpulse(impulse, relpos);
-	
 	}
-	
 
-	
-	updateFriction( step);
+	updateFriction(step);
 
-	
-	for (i=0;i<m_wheelInfo.size();i++)
+	for (i = 0; i < m_wheelInfo.size(); i++)
 	{
 		btWheelInfo& wheel = m_wheelInfo[i];
 		btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
-		btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos );
+		btVector3 vel = getRigidBody()->getVelocityInLocalPoint(relpos);
 
 		if (wheel.m_raycastInfo.m_isInContact)
 		{
-			const btTransform&	chassisWorldTransform = getChassisWorldTransform();
+			const btTransform& chassisWorldTransform = getChassisWorldTransform();
 
-			btVector3 fwd (
+			btVector3 fwd(
 				chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
 				chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
 				chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
@@ -346,99 +328,88 @@ void btRaycastVehicle::updateVehicle( btScalar step )
 			fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
 
 			btScalar proj2 = fwd.dot(vel);
-			
+
 			wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
 			wheel.m_rotation += wheel.m_deltaRotation;
-
-		} else
+		}
+		else
 		{
 			wheel.m_rotation += wheel.m_deltaRotation;
 		}
-		
-		wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact
 
+		wheel.m_deltaRotation *= btScalar(0.99);  //damping of rotation when not in contact
 	}
-
-
-
 }
 
-
-void	btRaycastVehicle::setSteeringValue(btScalar steering,int wheel)
+void btRaycastVehicle::setSteeringValue(btScalar steering, int wheel)
 {
-	btAssert(wheel>=0 && wheel < getNumWheels());
+	btAssert(wheel >= 0 && wheel < getNumWheels());
 
 	btWheelInfo& wheelInfo = getWheelInfo(wheel);
 	wheelInfo.m_steering = steering;
 }
 
-
-
-btScalar	btRaycastVehicle::getSteeringValue(int wheel) const
+btScalar btRaycastVehicle::getSteeringValue(int wheel) const
 {
 	return getWheelInfo(wheel).m_steering;
 }
 
-
-void	btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
+void btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
 {
-	btAssert(wheel>=0 && wheel < getNumWheels());
+	btAssert(wheel >= 0 && wheel < getNumWheels());
 	btWheelInfo& wheelInfo = getWheelInfo(wheel);
 	wheelInfo.m_engineForce = force;
 }
 
-
-const btWheelInfo&	btRaycastVehicle::getWheelInfo(int index) const
+const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const
 {
-	btAssert((index >= 0) && (index < 	getNumWheels()));
-	
+	btAssert((index >= 0) && (index < getNumWheels()));
+
 	return m_wheelInfo[index];
 }
 
-btWheelInfo&	btRaycastVehicle::getWheelInfo(int index) 
+btWheelInfo& btRaycastVehicle::getWheelInfo(int index)
 {
-	btAssert((index >= 0) && (index < 	getNumWheels()));
-	
+	btAssert((index >= 0) && (index < getNumWheels()));
+
 	return m_wheelInfo[index];
 }
 
-void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex)
+void btRaycastVehicle::setBrake(btScalar brake, int wheelIndex)
 {
-	btAssert((wheelIndex >= 0) && (wheelIndex < 	getNumWheels()));
+	btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels()));
 	getWheelInfo(wheelIndex).m_brake = brake;
 }
 
-
-void	btRaycastVehicle::updateSuspension(btScalar deltaTime)
+void btRaycastVehicle::updateSuspension(btScalar deltaTime)
 {
 	(void)deltaTime;
 
 	btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass();
-	
-	for (int w_it=0; w_it<getNumWheels(); w_it++)
+
+	for (int w_it = 0; w_it < getNumWheels(); w_it++)
 	{
-		btWheelInfo &wheel_info = m_wheelInfo[w_it];
-		
-		if ( wheel_info.m_raycastInfo.m_isInContact )
+		btWheelInfo& wheel_info = m_wheelInfo[w_it];
+
+		if (wheel_info.m_raycastInfo.m_isInContact)
 		{
 			btScalar force;
 			//	Spring
 			{
-				btScalar	susp_length			= wheel_info.getSuspensionRestLength();
-				btScalar	current_length = wheel_info.m_raycastInfo.m_suspensionLength;
+				btScalar susp_length = wheel_info.getSuspensionRestLength();
+				btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength;
 
 				btScalar length_diff = (susp_length - current_length);
 
-				force = wheel_info.m_suspensionStiffness
-					* length_diff * wheel_info.m_clippedInvContactDotSuspension;
+				force = wheel_info.m_suspensionStiffness * length_diff * wheel_info.m_clippedInvContactDotSuspension;
 			}
-		
+
 			// Damper
 			{
 				btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity;
 				{
-					btScalar	susp_damping;
-					if ( projected_rel_vel < btScalar(0.0) )
+					btScalar susp_damping;
+					if (projected_rel_vel < btScalar(0.0))
 					{
 						susp_damping = wheel_info.m_wheelsDampingCompression;
 					}
@@ -462,50 +433,43 @@ void	btRaycastVehicle::updateSuspension(btScalar deltaTime)
 			wheel_info.m_wheelsSuspensionForce = btScalar(0.0);
 		}
 	}
-
 }
 
-
 struct btWheelContactPoint
 {
 	btRigidBody* m_body0;
 	btRigidBody* m_body1;
-	btVector3	m_frictionPositionWorld;
-	btVector3	m_frictionDirectionWorld;
-	btScalar	m_jacDiagABInv;
-	btScalar	m_maxImpulse;
-
-
-	btWheelContactPoint(btRigidBody* body0,btRigidBody* body1,const btVector3& frictionPosWorld,const btVector3& frictionDirectionWorld, btScalar maxImpulse)
-		:m_body0(body0),
-		m_body1(body1),
-		m_frictionPositionWorld(frictionPosWorld),
-		m_frictionDirectionWorld(frictionDirectionWorld),
-		m_maxImpulse(maxImpulse)
+	btVector3 m_frictionPositionWorld;
+	btVector3 m_frictionDirectionWorld;
+	btScalar m_jacDiagABInv;
+	btScalar m_maxImpulse;
+
+	btWheelContactPoint(btRigidBody* body0, btRigidBody* body1, const btVector3& frictionPosWorld, const btVector3& frictionDirectionWorld, btScalar maxImpulse)
+		: m_body0(body0),
+		  m_body1(body1),
+		  m_frictionPositionWorld(frictionPosWorld),
+		  m_frictionDirectionWorld(frictionDirectionWorld),
+		  m_maxImpulse(maxImpulse)
 	{
-		btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
-		btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld);
-		btScalar	relaxation = 1.f;
-		m_jacDiagABInv = relaxation/(denom0+denom1);
+		btScalar denom0 = body0->computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld);
+		btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld);
+		btScalar relaxation = 1.f;
+		m_jacDiagABInv = relaxation / (denom0 + denom1);
 	}
-
-
-
 };
 
-btScalar calcRollingFriction(btWheelContactPoint& contactPoint);
-btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
+btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround);
+btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround)
 {
-
-	btScalar j1=0.f;
+	btScalar j1 = 0.f;
 
 	const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld;
 
-	btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition(); 
+	btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition();
 	btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition();
-	
-	btScalar maxImpulse  = contactPoint.m_maxImpulse;
-	
+
+	btScalar maxImpulse = contactPoint.m_maxImpulse;
+
 	btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1);
 	btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2);
 	btVector3 vel = vel1 - vel2;
@@ -513,252 +477,225 @@ btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
 	btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);
 
 	// calculate j that moves us to zero relative velocity
-	j1 = -vrel * contactPoint.m_jacDiagABInv;
+	j1 = -vrel * contactPoint.m_jacDiagABInv / btScalar(numWheelsOnGround);
 	btSetMin(j1, maxImpulse);
 	btSetMax(j1, -maxImpulse);
 
 	return j1;
 }
 
+btScalar sideFrictionStiffness2 = btScalar(1.0);
+void btRaycastVehicle::updateFriction(btScalar timeStep)
+{
+	//calculate the impulse, so that the wheels don't move sidewards
+	int numWheel = getNumWheels();
+	if (!numWheel)
+		return;
 
+	m_forwardWS.resize(numWheel);
+	m_axle.resize(numWheel);
+	m_forwardImpulse.resize(numWheel);
+	m_sideImpulse.resize(numWheel);
 
+	int numWheelsOnGround = 0;
 
-btScalar sideFrictionStiffness2 = btScalar(1.0);
-void	btRaycastVehicle::updateFriction(btScalar	timeStep)
-{
+	//collapse all those loops into one!
+	for (int i = 0; i < getNumWheels(); i++)
+	{
+		btWheelInfo& wheelInfo = m_wheelInfo[i];
+		class btRigidBody* groundObject = (class btRigidBody*)wheelInfo.m_raycastInfo.m_groundObject;
+		if (groundObject)
+			numWheelsOnGround++;
+		m_sideImpulse[i] = btScalar(0.);
+		m_forwardImpulse[i] = btScalar(0.);
+	}
 
-		//calculate the impulse, so that the wheels don't move sidewards
-		int numWheel = getNumWheels();
-		if (!numWheel)
-			return;
-
-		m_forwardWS.resize(numWheel);
-		m_axle.resize(numWheel);
-		m_forwardImpulse.resize(numWheel);
-		m_sideImpulse.resize(numWheel);
-		
-		int numWheelsOnGround = 0;
-	
-
-		//collapse all those loops into one!
-		for (int i=0;i<getNumWheels();i++)
+	{
+		for (int i = 0; i < getNumWheels(); i++)
 		{
 			btWheelInfo& wheelInfo = m_wheelInfo[i];
-			class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
-			if (groundObject)
-				numWheelsOnGround++;
-			m_sideImpulse[i] = btScalar(0.);
-			m_forwardImpulse[i] = btScalar(0.);
 
-		}
-	
-		{
-	
-			for (int i=0;i<getNumWheels();i++)
+			class btRigidBody* groundObject = (class btRigidBody*)wheelInfo.m_raycastInfo.m_groundObject;
+
+			if (groundObject)
 			{
+				const btTransform& wheelTrans = getWheelTransformWS(i);
 
-				btWheelInfo& wheelInfo = m_wheelInfo[i];
-					
-				class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
+				btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
+				m_axle[i] = -btVector3(
+					wheelBasis0[0][m_indexRightAxis],
+					wheelBasis0[1][m_indexRightAxis],
+					wheelBasis0[2][m_indexRightAxis]);
 
-				if (groundObject)
-				{
+				const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
+				btScalar proj = m_axle[i].dot(surfNormalWS);
+				m_axle[i] -= surfNormalWS * proj;
+				m_axle[i] = m_axle[i].normalize();
 
-					const btTransform& wheelTrans = getWheelTransformWS( i );
-
-					btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
-					m_axle[i] = btVector3(	
-						wheelBasis0[0][m_indexRightAxis],
-						wheelBasis0[1][m_indexRightAxis],
-						wheelBasis0[2][m_indexRightAxis]);
-					
-					const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
-					btScalar proj = m_axle[i].dot(surfNormalWS);
-					m_axle[i] -= surfNormalWS * proj;
-					m_axle[i] = m_axle[i].normalize();
-					
-					m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
-					m_forwardWS[i].normalize();
-
-				
-					resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
-							  *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
-							  btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep);
-
-					m_sideImpulse[i] *= sideFrictionStiffness2;
-						
-				}
-				
+				m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
+				m_forwardWS[i].normalize();
+
+				resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
+									   *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
+									   btScalar(0.), m_axle[i], m_sideImpulse[i], timeStep);
 
+				m_sideImpulse[i] *= sideFrictionStiffness2;
 			}
 		}
+	}
 
 	btScalar sideFactor = btScalar(1.);
 	btScalar fwdFactor = 0.5;
 
 	bool sliding = false;
 	{
-		for (int wheel =0;wheel <getNumWheels();wheel++)
+		for (int wheel = 0; wheel < getNumWheels(); wheel++)
 		{
 			btWheelInfo& wheelInfo = m_wheelInfo[wheel];
-			class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
+			class btRigidBody* groundObject = (class btRigidBody*)wheelInfo.m_raycastInfo.m_groundObject;
 
-			btScalar	rollingFriction = 0.f;
+			btScalar rollingFriction = 0.f;
 
 			if (groundObject)
 			{
 				if (wheelInfo.m_engineForce != 0.f)
 				{
-					rollingFriction = wheelInfo.m_engineForce* timeStep;
-				} else
+					rollingFriction = wheelInfo.m_engineForce * timeStep;
+				}
+				else
 				{
 					btScalar defaultRollingFrictionImpulse = 0.f;
 					btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
-					btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
-					rollingFriction = calcRollingFriction(contactPt);
+					btWheelContactPoint contactPt(m_chassisBody, groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, m_forwardWS[wheel], maxImpulse);
+					btAssert(numWheelsOnGround > 0);
+					rollingFriction = calcRollingFriction(contactPt, numWheelsOnGround);
 				}
 			}
 
 			//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
-			
-
-
 
 			m_forwardImpulse[wheel] = btScalar(0.);
-			m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
+			m_wheelInfo[wheel].m_skidInfo = btScalar(1.);
 
 			if (groundObject)
 			{
-				m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
-				
+				m_wheelInfo[wheel].m_skidInfo = btScalar(1.);
+
 				btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
 				btScalar maximpSide = maximp;
 
 				btScalar maximpSquared = maximp * maximpSide;
-			
 
-				m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
+				m_forwardImpulse[wheel] = rollingFriction;  //wheelInfo.m_engineForce* timeStep;
 
-				btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor;
-				btScalar y = (m_sideImpulse[wheel] ) * sideFactor;
-				
-				btScalar impulseSquared = (x*x + y*y);
+				btScalar x = (m_forwardImpulse[wheel]) * fwdFactor;
+				btScalar y = (m_sideImpulse[wheel]) * sideFactor;
+
+				btScalar impulseSquared = (x * x + y * y);
 
 				if (impulseSquared > maximpSquared)
 				{
 					sliding = true;
-					
+
 					btScalar factor = maximp / btSqrt(impulseSquared);
-					
+
 					m_wheelInfo[wheel].m_skidInfo *= factor;
 				}
-			} 
-
+			}
 		}
 	}
 
-	
-
-
-		if (sliding)
+	if (sliding)
+	{
+		for (int wheel = 0; wheel < getNumWheels(); wheel++)
 		{
-			for (int wheel = 0;wheel < getNumWheels(); wheel++)
+			if (m_sideImpulse[wheel] != btScalar(0.))
 			{
-				if (m_sideImpulse[wheel] != btScalar(0.))
+				if (m_wheelInfo[wheel].m_skidInfo < btScalar(1.))
 				{
-					if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.))
-					{
-						m_forwardImpulse[wheel] *=	m_wheelInfo[wheel].m_skidInfo;
-						m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
-					}
+					m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
+					m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
 				}
 			}
 		}
+	}
 
-		// apply the impulses
+	// apply the impulses
+	{
+		for (int wheel = 0; wheel < getNumWheels(); wheel++)
 		{
-			for (int wheel = 0;wheel<getNumWheels() ; wheel++)
-			{
-				btWheelInfo& wheelInfo = m_wheelInfo[wheel];
+			btWheelInfo& wheelInfo = m_wheelInfo[wheel];
 
-				btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - 
-						m_chassisBody->getCenterOfMassPosition();
+			btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
+								m_chassisBody->getCenterOfMassPosition();
 
-				if (m_forwardImpulse[wheel] != btScalar(0.))
-				{
-					m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos);
-				}
-				if (m_sideImpulse[wheel] != btScalar(0.))
-				{
-					class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
+			if (m_forwardImpulse[wheel] != btScalar(0.))
+			{
+				m_chassisBody->applyImpulse(m_forwardWS[wheel] * (m_forwardImpulse[wheel]), rel_pos);
+			}
+			if (m_sideImpulse[wheel] != btScalar(0.))
+			{
+				class btRigidBody* groundObject = (class btRigidBody*)m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
 
-					btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - 
-						groundObject->getCenterOfMassPosition();
+				btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
+									 groundObject->getCenterOfMassPosition();
 
-					
-					btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
+				btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
 
-#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
-					btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
-					rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence));
+#if defined ROLLING_INFLUENCE_FIX  // fix. It only worked if car's up was along Y - VT.
+				btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
+				rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f - wheelInfo.m_rollInfluence));
 #else
-					rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
+				rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
 #endif
-					m_chassisBody->applyImpulse(sideImp,rel_pos);
+				m_chassisBody->applyImpulse(sideImp, rel_pos);
 
-					//apply friction impulse on the ground
-					groundObject->applyImpulse(-sideImp,rel_pos2);
-				}
+				//apply friction impulse on the ground
+				groundObject->applyImpulse(-sideImp, rel_pos2);
 			}
 		}
-
-	
+	}
 }
 
-
-
-void	btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer)
+void btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer)
 {
-
-	for (int v=0;v<this->getNumWheels();v++)
+	for (int v = 0; v < this->getNumWheels(); v++)
 	{
-		btVector3 wheelColor(0,1,1);
+		btVector3 wheelColor(0, 1, 1);
 		if (getWheelInfo(v).m_raycastInfo.m_isInContact)
 		{
-			wheelColor.setValue(0,0,1);
-		} else
+			wheelColor.setValue(0, 0, 1);
+		}
+		else
 		{
-			wheelColor.setValue(1,0,1);
+			wheelColor.setValue(1, 0, 1);
 		}
 
 		btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin();
 
-		btVector3 axle = btVector3(	
+		btVector3 axle = btVector3(
 			getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()],
 			getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()],
 			getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]);
 
 		//debug wheels (cylinders)
-		debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
-		debugDrawer->drawLine(wheelPosWS,getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
-
+		debugDrawer->drawLine(wheelPosWS, wheelPosWS + axle, wheelColor);
+		debugDrawer->drawLine(wheelPosWS, getWheelInfo(v).m_raycastInfo.m_contactPointWS, wheelColor);
 	}
 }
 
-
-void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result)
+void* btDefaultVehicleRaycaster::castRay(const btVector3& from, const btVector3& to, btVehicleRaycasterResult& result)
 {
-//	RayResultCallback& resultCallback;
+	//	RayResultCallback& resultCallback;
 
-	btCollisionWorld::ClosestRayResultCallback rayCallback(from,to);
+	btCollisionWorld::ClosestRayResultCallback rayCallback(from, to);
 
 	m_dynamicsWorld->rayTest(from, to, rayCallback);
 
 	if (rayCallback.hasHit())
 	{
-		
 		const btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
-        if (body && body->hasContactResponse())
+		if (body && body->hasContactResponse())
 		{
 			result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
 			result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
@@ -769,4 +706,3 @@ void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3&
 	}
 	return 0;
 }
-
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
index 82d44c73e05..99d6894e56e 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
@@ -19,127 +19,116 @@ class btDynamicsWorld;
 #include "btWheelInfo.h"
 #include "BulletDynamics/Dynamics/btActionInterface.h"
 
-class btVehicleTuning;
+//class btVehicleTuning;
 
 ///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle.
 class btRaycastVehicle : public btActionInterface
 {
+	btAlignedObjectArray<btVector3> m_forwardWS;
+	btAlignedObjectArray<btVector3> m_axle;
+	btAlignedObjectArray<btScalar> m_forwardImpulse;
+	btAlignedObjectArray<btScalar> m_sideImpulse;
 
-		btAlignedObjectArray<btVector3>	m_forwardWS;
-		btAlignedObjectArray<btVector3>	m_axle;
-		btAlignedObjectArray<btScalar>	m_forwardImpulse;
-		btAlignedObjectArray<btScalar>	m_sideImpulse;
-	
-		///backwards compatibility
-		int	m_userConstraintType;
-		int	m_userConstraintId;
+	///backwards compatibility
+	int m_userConstraintType;
+	int m_userConstraintId;
 
 public:
 	class btVehicleTuning
+	{
+	public:
+		btVehicleTuning()
+			: m_suspensionStiffness(btScalar(5.88)),
+			  m_suspensionCompression(btScalar(0.83)),
+			  m_suspensionDamping(btScalar(0.88)),
+			  m_maxSuspensionTravelCm(btScalar(500.)),
+			  m_frictionSlip(btScalar(10.5)),
+			  m_maxSuspensionForce(btScalar(6000.))
 		{
-			public:
-
-			btVehicleTuning()
-				:m_suspensionStiffness(btScalar(5.88)),
-				m_suspensionCompression(btScalar(0.83)),
-				m_suspensionDamping(btScalar(0.88)),
-				m_maxSuspensionTravelCm(btScalar(500.)),
-				m_frictionSlip(btScalar(10.5)),
-				m_maxSuspensionForce(btScalar(6000.))
-			{
-			}
-			btScalar	m_suspensionStiffness;
-			btScalar	m_suspensionCompression;
-			btScalar	m_suspensionDamping;
-			btScalar	m_maxSuspensionTravelCm;
-			btScalar	m_frictionSlip;
-			btScalar	m_maxSuspensionForce;
-
-		};
-private:
+		}
+		btScalar m_suspensionStiffness;
+		btScalar m_suspensionCompression;
+		btScalar m_suspensionDamping;
+		btScalar m_maxSuspensionTravelCm;
+		btScalar m_frictionSlip;
+		btScalar m_maxSuspensionForce;
+	};
 
-	btVehicleRaycaster*	m_vehicleRaycaster;
-	btScalar		m_pitchControl;
-	btScalar	m_steeringValue; 
+private:
+	btVehicleRaycaster* m_vehicleRaycaster;
+	btScalar m_pitchControl;
+	btScalar m_steeringValue;
 	btScalar m_currentVehicleSpeedKmHour;
 
 	btRigidBody* m_chassisBody;
 
 	int m_indexRightAxis;
 	int m_indexUpAxis;
-	int	m_indexForwardAxis;
+	int m_indexForwardAxis;
 
 	void defaultInit(const btVehicleTuning& tuning);
 
 public:
-
 	//constructor to create a car from an existing rigidbody
-	btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,	btVehicleRaycaster* raycaster );
-
-	virtual ~btRaycastVehicle() ;
+	btRaycastVehicle(const btVehicleTuning& tuning, btRigidBody* chassis, btVehicleRaycaster* raycaster);
 
+	virtual ~btRaycastVehicle();
 
 	///btActionInterface interface
-	virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step)
+	virtual void updateAction(btCollisionWorld* collisionWorld, btScalar step)
 	{
-        (void) collisionWorld;
+		(void)collisionWorld;
 		updateVehicle(step);
 	}
-	
 
 	///btActionInterface interface
-	void	debugDraw(btIDebugDraw* debugDrawer);
-			
+	void debugDraw(btIDebugDraw* debugDrawer);
+
 	const btTransform& getChassisWorldTransform() const;
-	
+
 	btScalar rayCast(btWheelInfo& wheel);
 
 	virtual void updateVehicle(btScalar step);
-	
-	
+
 	void resetSuspension();
 
-	btScalar	getSteeringValue(int wheel) const;
+	btScalar getSteeringValue(int wheel) const;
 
-	void	setSteeringValue(btScalar steering,int wheel);
+	void setSteeringValue(btScalar steering, int wheel);
 
+	void applyEngineForce(btScalar force, int wheel);
 
-	void	applyEngineForce(btScalar force, int wheel);
+	const btTransform& getWheelTransformWS(int wheelIndex) const;
 
-	const btTransform&	getWheelTransformWS( int wheelIndex ) const;
+	void updateWheelTransform(int wheelIndex, bool interpolatedTransform = true);
 
-	void	updateWheelTransform( int wheelIndex, bool interpolatedTransform = true );
-	
-//	void	setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth);
+	//	void	setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth);
 
-	btWheelInfo&	addWheel( const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS,btScalar suspensionRestLength,btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel);
+	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 int (m_wheelInfo.size());
+	inline int getNumWheels() const
+	{
+		return int(m_wheelInfo.size());
 	}
-	
-	btAlignedObjectArray<btWheelInfo>	m_wheelInfo;
 
+	btAlignedObjectArray<btWheelInfo> m_wheelInfo;
 
-	const btWheelInfo&	getWheelInfo(int index) const;
+	const btWheelInfo& getWheelInfo(int index) const;
 
-	btWheelInfo&	getWheelInfo(int index);
+	btWheelInfo& getWheelInfo(int index);
 
-	void	updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true);
+	void updateWheelTransformsWS(btWheelInfo& wheel, bool interpolatedTransform = true);
 
-	
-	void setBrake(btScalar brake,int wheelIndex);
+	void setBrake(btScalar brake, int wheelIndex);
 
-	void	setPitchControl(btScalar pitch)
+	void setPitchControl(btScalar pitch)
 	{
 		m_pitchControl = pitch;
 	}
-	
-	void	updateSuspension(btScalar deltaTime);
-
-	virtual void	updateFriction(btScalar	timeStep);
 
+	void updateSuspension(btScalar deltaTime);
 
+	virtual void updateFriction(btScalar timeStep);
 
 	inline btRigidBody* getRigidBody()
 	{
@@ -151,7 +140,7 @@ public:
 		return m_chassisBody;
 	}
 
-	inline int	getRightAxis() const
+	inline int getRightAxis() const
 	{
 		return m_indexRightAxis;
 	}
@@ -165,46 +154,44 @@ public:
 		return m_indexForwardAxis;
 	}
 
-	
 	///Worldspace forward vector
 	btVector3 getForwardVector() const
 	{
-		const btTransform& chassisTrans = getChassisWorldTransform(); 
+		const btTransform& chassisTrans = getChassisWorldTransform();
 
-		btVector3 forwardW ( 
-			  chassisTrans.getBasis()[0][m_indexForwardAxis], 
-			  chassisTrans.getBasis()[1][m_indexForwardAxis], 
-			  chassisTrans.getBasis()[2][m_indexForwardAxis]); 
+		btVector3 forwardW(
+			chassisTrans.getBasis()[0][m_indexForwardAxis],
+			chassisTrans.getBasis()[1][m_indexForwardAxis],
+			chassisTrans.getBasis()[2][m_indexForwardAxis]);
 
 		return forwardW;
 	}
 
 	///Velocity of vehicle (positive if velocity vector has same direction as foward vector)
-	btScalar	getCurrentSpeedKmHour() const
+	btScalar getCurrentSpeedKmHour() const
 	{
 		return m_currentVehicleSpeedKmHour;
 	}
 
-	virtual void	setCoordinateSystem(int rightIndex,int upIndex,int forwardIndex)
+	virtual void setCoordinateSystem(int rightIndex, int upIndex, int forwardIndex)
 	{
 		m_indexRightAxis = rightIndex;
 		m_indexUpAxis = upIndex;
 		m_indexForwardAxis = forwardIndex;
 	}
 
-
 	///backwards compatibility
 	int getUserConstraintType() const
 	{
-		return m_userConstraintType ;
+		return m_userConstraintType;
 	}
 
-	void	setUserConstraintType(int userConstraintType)
+	void setUserConstraintType(int userConstraintType)
 	{
 		m_userConstraintType = userConstraintType;
 	};
 
-	void	setUserConstraintId(int uid)
+	void setUserConstraintId(int uid)
 	{
 		m_userConstraintId = uid;
 	}
@@ -213,22 +200,19 @@ public:
 	{
 		return m_userConstraintId;
 	}
-
 };
 
 class btDefaultVehicleRaycaster : public btVehicleRaycaster
 {
-	btDynamicsWorld*	m_dynamicsWorld;
+	btDynamicsWorld* m_dynamicsWorld;
+
 public:
 	btDefaultVehicleRaycaster(btDynamicsWorld* world)
-		:m_dynamicsWorld(world)
+		: m_dynamicsWorld(world)
 	{
 	}
 
-	virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result);
-
+	virtual void* castRay(const btVector3& from, const btVector3& to, btVehicleRaycasterResult& result);
 };
 
-
-#endif //BT_RAYCASTVEHICLE_H
-
+#endif  //BT_RAYCASTVEHICLE_H
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h b/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h
index 3cc909c6530..2c44ce546c7 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h
@@ -16,20 +16,18 @@
 /// btVehicleRaycaster is provides interface for between vehicle simulation and raycasting
 struct btVehicleRaycaster
 {
-virtual ~btVehicleRaycaster()
-{
-}
+	virtual ~btVehicleRaycaster()
+	{
+	}
 	struct btVehicleRaycasterResult
 	{
-		btVehicleRaycasterResult() :m_distFraction(btScalar(-1.)){};
-		btVector3	m_hitPointInWorld;
-		btVector3	m_hitNormalInWorld;
-		btScalar	m_distFraction;
+		btVehicleRaycasterResult() : m_distFraction(btScalar(-1.)){};
+		btVector3 m_hitPointInWorld;
+		btVector3 m_hitNormalInWorld;
+		btScalar m_distFraction;
 	};
 
-	virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) = 0;
-
+	virtual void* castRay(const btVector3& from, const btVector3& to, btVehicleRaycasterResult& result) = 0;
 };
 
-#endif //BT_VEHICLE_RAYCASTER_H
-
+#endif  //BT_VEHICLE_RAYCASTER_H
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp
index ef93c16fffc..d5c12f223b5 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp
@@ -9,30 +9,26 @@
  * It is provided "as is" without express or implied warranty.
 */
 #include "btWheelInfo.h"
-#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity
-
+#include "BulletDynamics/Dynamics/btRigidBody.h"  // for pointvelocity
 
 btScalar btWheelInfo::getSuspensionRestLength() const
 {
-
 	return m_suspensionRestLength1;
-
 }
 
-void	btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo)
+void btWheelInfo::updateWheel(const btRigidBody& chassis, RaycastInfo& raycastInfo)
 {
 	(void)raycastInfo;
 
-	
 	if (m_raycastInfo.m_isInContact)
 
 	{
-		btScalar	project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS );
-		btVector3	 chassis_velocity_at_contactPoint;
+		btScalar project = m_raycastInfo.m_contactNormalWS.dot(m_raycastInfo.m_wheelDirectionWS);
+		btVector3 chassis_velocity_at_contactPoint;
 		btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition();
-		chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos );
-		btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
-		if ( project >= btScalar(-0.1))
+		chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint(relpos);
+		btScalar projVel = m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint);
+		if (project >= btScalar(-0.1))
 		{
 			m_suspensionRelativeVelocity = btScalar(0.0);
 			m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
@@ -43,10 +39,9 @@ void	btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInf
 			m_suspensionRelativeVelocity = projVel * inv;
 			m_clippedInvContactDotSuspension = inv;
 		}
-		
 	}
 
-	else	// Not in contact : position wheel in a nice (rest length) position
+	else  // Not in contact : position wheel in a nice (rest length) position
 	{
 		m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength();
 		m_suspensionRelativeVelocity = btScalar(0.0);
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h
index f916053ecac..af88b8ff837 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h
@@ -18,20 +18,19 @@ class btRigidBody;
 
 struct btWheelInfoConstructionInfo
 {
-	btVector3	m_chassisConnectionCS;
-	btVector3	m_wheelDirectionCS;
-	btVector3	m_wheelAxleCS;
-	btScalar	m_suspensionRestLength;
-	btScalar	m_maxSuspensionTravelCm;
-	btScalar	m_wheelRadius;
-	
-	btScalar		m_suspensionStiffness;
-	btScalar		m_wheelsDampingCompression;
-	btScalar		m_wheelsDampingRelaxation;
-	btScalar		m_frictionSlip;
-	btScalar		m_maxSuspensionForce;
+	btVector3 m_chassisConnectionCS;
+	btVector3 m_wheelDirectionCS;
+	btVector3 m_wheelAxleCS;
+	btScalar m_suspensionRestLength;
+	btScalar m_maxSuspensionTravelCm;
+	btScalar m_wheelRadius;
+
+	btScalar m_suspensionStiffness;
+	btScalar m_wheelsDampingCompression;
+	btScalar m_wheelsDampingRelaxation;
+	btScalar m_frictionSlip;
+	btScalar m_maxSuspensionForce;
 	bool m_bIsFrontWheel;
-	
 };
 
 /// btWheelInfo contains information per wheel about friction and suspension.
@@ -40,49 +39,50 @@ struct btWheelInfo
 	struct RaycastInfo
 	{
 		//set by raycaster
-		btVector3	m_contactNormalWS;//contactnormal
-		btVector3	m_contactPointWS;//raycast hitpoint
-		btScalar	m_suspensionLength;
-		btVector3	m_hardPointWS;//raycast starting point
-		btVector3	m_wheelDirectionWS; //direction in worldspace
-		btVector3	m_wheelAxleWS; // axle in worldspace
-		bool		m_isInContact;
-		void*		m_groundObject; //could be general void* ptr
+		btVector3 m_contactNormalWS;  //contactnormal
+		btVector3 m_contactPointWS;   //raycast hitpoint
+		btScalar m_suspensionLength;
+		btVector3 m_hardPointWS;       //raycast starting point
+		btVector3 m_wheelDirectionWS;  //direction in worldspace
+		btVector3 m_wheelAxleWS;       // axle in worldspace
+		bool m_isInContact;
+		void* m_groundObject;  //could be general void* ptr
 	};
 
-	RaycastInfo	m_raycastInfo;
+	RaycastInfo m_raycastInfo;
 
-	btTransform	m_worldTransform;
-	
-	btVector3	m_chassisConnectionPointCS; //const
-	btVector3	m_wheelDirectionCS;//const
-	btVector3	m_wheelAxleCS; // const or modified by steering
-	btScalar	m_suspensionRestLength1;//const
-	btScalar	m_maxSuspensionTravelCm;
+	btTransform m_worldTransform;
+
+	btVector3 m_chassisConnectionPointCS;  //const
+	btVector3 m_wheelDirectionCS;          //const
+	btVector3 m_wheelAxleCS;               // const or modified by steering
+	btScalar m_suspensionRestLength1;      //const
+	btScalar m_maxSuspensionTravelCm;
 	btScalar getSuspensionRestLength() const;
-	btScalar	m_wheelsRadius;//const
-	btScalar	m_suspensionStiffness;//const
-	btScalar	m_wheelsDampingCompression;//const
-	btScalar	m_wheelsDampingRelaxation;//const
-	btScalar	m_frictionSlip;
-	btScalar	m_steering;
-	btScalar	m_rotation;
-	btScalar	m_deltaRotation;
-	btScalar	m_rollInfluence;
-	btScalar	m_maxSuspensionForce;
-
-	btScalar	m_engineForce;
-
-	btScalar	m_brake;
-	
+	btScalar m_wheelsRadius;              //const
+	btScalar m_suspensionStiffness;       //const
+	btScalar m_wheelsDampingCompression;  //const
+	btScalar m_wheelsDampingRelaxation;   //const
+	btScalar m_frictionSlip;
+	btScalar m_steering;
+	btScalar m_rotation;
+	btScalar m_deltaRotation;
+	btScalar m_rollInfluence;
+	btScalar m_maxSuspensionForce;
+
+	btScalar m_engineForce;
+
+	btScalar m_brake;
+
 	bool m_bIsFrontWheel;
-	
-	void*		m_clientInfo;//can be used to store pointer to sync transforms...
+
+	void* m_clientInfo;  //can be used to store pointer to sync transforms...
+
+	btWheelInfo() {}
 
 	btWheelInfo(btWheelInfoConstructionInfo& ci)
 
 	{
-
 		m_suspensionRestLength1 = ci.m_suspensionRestLength;
 		m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm;
 
@@ -102,18 +102,15 @@ struct btWheelInfo
 		m_rollInfluence = btScalar(0.1);
 		m_bIsFrontWheel = ci.m_bIsFrontWheel;
 		m_maxSuspensionForce = ci.m_maxSuspensionForce;
-
 	}
 
-	void	updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo);
+	void updateWheel(const btRigidBody& chassis, RaycastInfo& raycastInfo);
 
-	btScalar	m_clippedInvContactDotSuspension;
-	btScalar	m_suspensionRelativeVelocity;
+	btScalar m_clippedInvContactDotSuspension;
+	btScalar m_suspensionRelativeVelocity;
 	//calculated by suspension
-	btScalar	m_wheelsSuspensionForce;
-	btScalar	m_skidInfo;
-
+	btScalar m_wheelsSuspensionForce;
+	btScalar m_skidInfo;
 };
 
-#endif //BT_WHEEL_INFO_H
-
+#endif  //BT_WHEEL_INFO_H