Update Bullet to the latest commit 126b676

12 files changed, 1840 insertions, 2072 deletions

diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btActionInterface.h b/thirdparty/bullet/BulletDynamics/Dynamics/btActionInterface.h
index e1fea3a49c..b5cac56cdc 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btActionInterface.h
+++ b/thirdparty/bullet/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/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index b9944c138b..dfbbdb154f 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/thirdparty/bullet/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,12 +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);
 
-
 	int numSimulationSubSteps = 0;
 
 	if (maxSubSteps)
@@ -412,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;
@@ -425,7 +406,8 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 		{
 			numSimulationSubSteps = 0;
 			maxSubSteps = 0;
-		} else
+		}
+		else
 		{
 			numSimulationSubSteps = 1;
 			maxSubSteps = 1;
@@ -435,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();
 	}
@@ -465,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);
 	}
 
@@ -488,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());
 
@@ -513,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, int collisionFilterGroup, 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)
@@ -552,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);
 	}
@@ -571,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());
-		int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
-		int collisionFilterMask = isDynamic? 	int(btBroadphaseProxy::AllFilter) : 	int(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, int group, int 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);
 	}
@@ -597,31 +573,29 @@ void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mas
 		{
 			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)
@@ -633,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);
@@ -666,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())
@@ -775,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());
 				}
 			}
 		}
@@ -783,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,13 +801,13 @@ public:
 		if (!ClosestConvexResultCallback::needsCollision(proxy0))
 			return false;
 
-		btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
+		btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject;
 
-		if(!m_dispatcher->needsCollision(m_me, otherObj))
+		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)
@@ -872,28 +833,24 @@ public:
 
 		return false;
 	}
-
-
 };
 
 ///internal debugging variable. this value shouldn't be too high
-int gNumClampedCcdMotions=0;
-
+int gNumClampedCcdMotions = 0;
 
-void btDiscreteDynamicsWorld::createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep)
+void btDiscreteDynamicsWorld::createPredictiveContactsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep)
 {
 	btTransform predictedTrans;
-	for ( int i=0;i<numBodies;i++)
+	for (int i = 0; i < numBodies; 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)
 			{
@@ -905,60 +862,55 @@ void btDiscreteDynamicsWorld::createPredictiveContactsInternal( btRigidBody** bo
 					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);
-                        btMutexLock( &m_predictiveManifoldsMutex );
+						btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body, sweepResults.m_hitCollisionObject);
+						btMutexLock(&m_predictiveManifoldsMutex);
 						m_predictiveManifolds.push_back(manifold);
-                        btMutexUnlock( &m_predictiveManifoldsMutex );
+						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 = gCalculateCombinedFrictionCallback(body,sweepResults.m_hitCollisionObject);
+						pt.m_combinedFriction = gCalculateCombinedFrictionCallback(body, sweepResults.m_hitCollisionObject);
 						pt.m_positionWorldOnA = body->getWorldTransform().getOrigin();
 						pt.m_positionWorldOnB = worldPointB;
-
 					}
 				}
 			}
@@ -968,42 +920,39 @@ void btDiscreteDynamicsWorld::createPredictiveContactsInternal( btRigidBody** bo
 
 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();
+	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 );
-    }
+	releasePredictiveContacts();
+	if (m_nonStaticRigidBodies.size() > 0)
+	{
+		createPredictiveContactsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep);
+	}
 }
 
-void btDiscreteDynamicsWorld::integrateTransformsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep )
+void btDiscreteDynamicsWorld::integrateTransformsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep)
 {
 	btTransform predictedTrans;
-	for (int i=0;i<numBodies;i++)
+	for (int i = 0; i < numBodies; 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)
 			{
@@ -1015,43 +964,40 @@ void btDiscreteDynamicsWorld::integrateTransformsInternal( btRigidBody** bodies,
 					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();
@@ -1078,50 +1024,45 @@ void btDiscreteDynamicsWorld::integrateTransformsInternal( btRigidBody** bodies,
 						//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);
-    }
+	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
+	///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 = 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();
@@ -1130,23 +1071,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())
@@ -1155,179 +1092,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));
-
-						if (i%(nSegments/8) == 0)
-							getDebugDrawer()->drawLine(tr.getOrigin(), 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));
 
-						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:
 		{
@@ -1359,9 +1288,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;
@@ -1383,42 +1312,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;
@@ -1430,8 +1355,7 @@ btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
 	return m_constraintSolver;
 }
 
-
-int		btDiscreteDynamicsWorld::getNumConstraints() const
+int btDiscreteDynamicsWorld::getNumConstraints() const
 {
 	return int(m_constraints.size());
 }
@@ -1444,93 +1368,87 @@ 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;
-
-		// Fill padding with zeros to appease msan.
-		memset(worldInfo->m_solverInfo.m_padding, 0, sizeof(worldInfo->m_solverInfo.m_padding));
+	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;
+
+	// Fill padding with zeros to appease msan.
+	memset(worldInfo->m_solverInfo.m_padding, 0, sizeof(worldInfo->m_solverInfo.m_padding));
 
 #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);
@@ -1543,4 +1461,3 @@ void	btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
 
 	serializer->finishSerialization();
 }
-
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
index b0d19f48a3..7fe9619213 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.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_DISCRETE_DYNAMICS_WORLD_H
 #define BT_DISCRETE_DYNAMICS_WORLD_H
 
@@ -32,159 +31,153 @@ 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;
-    btSpinMutex m_predictiveManifoldsMutex;  // used to synchronize threads creating predictive contacts
+	bool m_latencyMotionStateInterpolation;
 
-	virtual void	predictUnconstraintMotion(btScalar timeStep);
-	
-    void integrateTransformsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep );  // can be called in parallel
-	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	updateActions(btScalar timeStep);
+	void integrateTransformsInternal(btRigidBody * *bodies, int numBodies, btScalar timeStep);  // can be called in parallel
+	virtual void integrateTransforms(btScalar timeStep);
 
-	void	startProfiling(btScalar timeStep);
+	virtual void calculateSimulationIslands();
 
-	virtual void	internalSingleStepSimulation( btScalar timeStep);
+	virtual void solveConstraints(btContactSolverInfo & solverInfo);
 
-    void releasePredictiveContacts();
-    void createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep );  // can be called in parallel
-	virtual void	createPredictiveContacts(btScalar timeStep);
+	virtual void updateActivationState(btScalar timeStep);
 
-	virtual void	saveKinematicState(btScalar timeStep);
+	void updateActions(btScalar timeStep);
 
-	void	serializeRigidBodies(btSerializer* serializer);
+	void startProfiling(btScalar timeStep);
 
-	void	serializeDynamicsWorldInfo(btSerializer* serializer);
+	virtual void internalSingleStepSimulation(btScalar timeStep);
 
-public:
+	void releasePredictiveContacts();
+	void createPredictiveContactsInternal(btRigidBody * *bodies, int numBodies, btScalar timeStep);  // can be called in parallel
+	virtual void createPredictiveContacts(btScalar timeStep);
 
+	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 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*);
 
-	virtual void	removeAction(btActionInterface*);
-	
-	btSimulationIslandManager*	getSimulationIslandManager()
+	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, int collisionFilterGroup=btBroadphaseProxy::StaticFilter, 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, int group, int 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
@@ -194,15 +187,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;
 	}
@@ -215,18 +208,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;
 	}
@@ -236,4 +229,4 @@ public:
 	}
 };
 
-#endif //BT_DISCRETE_DYNAMICS_WORLD_H
+#endif  //BT_DISCRETE_DYNAMICS_WORLD_H
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp
index d705bf2381..8207b47135 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btDiscreteDynamicsWorldMt.h"
 
 //collision detection
@@ -38,148 +37,139 @@ 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"
 
 #include "LinearMath/btSerializer.h"
 
-
-
 ///
 /// btConstraintSolverPoolMt
 ///
 
 btConstraintSolverPoolMt::ThreadSolver* btConstraintSolverPoolMt::getAndLockThreadSolver()
 {
-    int i = 0;
+	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;
+	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 )
+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();
-    }
+	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 )
+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 );
+	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 )
+btConstraintSolverPoolMt::btConstraintSolverPoolMt(btConstraintSolver** solvers, int numSolvers)
 {
-    init( solvers, 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;
-    }
+	// 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
-)
+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;
+	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();
-    }
+	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* constraintSolver,
-    btConstraintSolver* constraintSolverMt,
-    btCollisionConfiguration* collisionConfiguration
-)
-: btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
+													 btBroadphaseInterface* pairCache,
+													 btConstraintSolverPoolMt* solverPool,
+													 btConstraintSolver* constraintSolverMt,
+													 btCollisionConfiguration* collisionConfiguration)
+	: btDiscreteDynamicsWorld(dispatcher, pairCache, solverPool, collisionConfiguration)
 {
 	if (m_ownsIslandManager)
 	{
 		m_islandManager->~btSimulationIslandManager();
-		btAlignedFree( m_islandManager);
+		btAlignedFree(m_islandManager);
 	}
 	{
-		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt),16);
+		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt), 16);
 		btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt();
-        im->setMinimumSolverBatchSize( m_solverInfo.m_minimumSolverBatchSize );
-        m_islandManager = im;
+		im->setMinimumSolverBatchSize(m_solverInfo.m_minimumSolverBatchSize);
+		m_islandManager = im;
 	}
-    m_constraintSolverMt = constraintSolverMt;
+	m_constraintSolverMt = constraintSolverMt;
 }
 
-
 btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt()
 {
 }
 
-
 void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo)
 {
 	BT_PROFILE("solveConstraints");
@@ -187,92 +177,87 @@ void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo
 	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 );
+	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;
+	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 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 )
+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 );
-    }
+	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 )
+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 );
-    }
+	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 )
+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 );
-    }
+	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 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;
+	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/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h
index 667fe5800e..dccf35d7a7 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.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_DISCRETE_DYNAMICS_WORLD_MT_H
 #define BT_DISCRETE_DYNAMICS_WORLD_MT_H
 
@@ -21,7 +20,6 @@ subject to the following restrictions:
 #include "btSimulationIslandManagerMt.h"
 #include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
 
-
 ///
 /// btConstraintSolverPoolMt - masquerades as a constraint solver, but really it is a threadsafe pool of them.
 ///
@@ -34,46 +32,43 @@ subject to the following restrictions:
 class btConstraintSolverPoolMt : public btConstraintSolver
 {
 public:
-    // create the solvers for me
-    explicit btConstraintSolverPoolMt( int numSolvers );
+	// create the solvers for me
+	explicit btConstraintSolverPoolMt(int numSolvers);
 
-    // pass in fully constructed solvers (destructor will delete them)
-    btConstraintSolverPoolMt( btConstraintSolver** solvers, int numSolvers );
+	// pass in fully constructed solvers (destructor will delete them)
+	btConstraintSolverPoolMt(btConstraintSolver** solvers, int numSolvers);
 
-    virtual ~btConstraintSolverPoolMt();
+	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;
+	///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; }
+	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 );
+	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.
@@ -84,53 +79,53 @@ private:
 ///     - integrateTransforms
 ///     - createPredictiveContacts
 ///
-ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld
+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;
+	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* constraintSolver,   // Note this should be a solver-pool for multi-threading
-        btConstraintSolver* constraintSolverMt,    // single multi-threaded solver for large islands (or NULL)
-        btCollisionConfiguration* collisionConfiguration
-    );
+	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;
+	virtual int stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) BT_OVERRIDE;
 };
 
-#endif //BT_DISCRETE_DYNAMICS_WORLD_H
+#endif  //BT_DISCRETE_DYNAMICS_WORLD_H
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h b/thirdparty/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h
index 42d8fc0de3..eadd8c12e7 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h
@@ -24,150 +24,150 @@ 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_SOFT_MULTIBODY_DYNAMICS_WORLD=6
+	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
 };
 
 ///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, int group, int 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;
+	}
 
-		void*	getWorldUserInfo() const
-		{
-			return m_worldUserInfo;
-		}
+	virtual const btTypedConstraint* getConstraint(int index) const
+	{
+		(void)index;
+		return 0;
+	}
+
+	virtual btDynamicsWorldType getWorldType() const = 0;
+
+	virtual void clearForces() = 0;
 
-		btContactSolverInfo& getSolverInfo()
+	/// 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_solverInfo;
+			m_internalPreTickCallback = cb;
 		}
-
-		const btContactSolverInfo& getSolverInfo() const
+		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/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp
index ca0714fcfa..f4bcabada2 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp
+++ b/thirdparty/bullet/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,48 +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_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.))
@@ -116,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))
 	{
@@ -143,22 +134,14 @@ 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));
 }
 
-
-
-
 ///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
@@ -168,8 +151,8 @@ void			btRigidBody::applyDamping(btScalar timeStep)
 	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));
 #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)
@@ -182,7 +165,6 @@ void			btRigidBody::applyDamping(btScalar timeStep)
 			m_angularVelocity *= m_additionalDampingFactor;
 			m_linearVelocity *= m_additionalDampingFactor;
 		}
-	
 
 		btScalar speed = m_linearVelocity.length();
 		if (speed < m_linearDamping)
@@ -191,10 +173,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.));
 			}
 		}
 
@@ -205,30 +188,28 @@ 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::proceedToTransform(const btTransform& newTrans)
 {
-	setCenterOfMassTransform( newTrans );
+	setCenterOfMassTransform(newTrans);
 }
-	
 
 void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
 {
@@ -236,7 +217,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;
@@ -244,50 +226,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;
 }
 
@@ -295,58 +272,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.
@@ -361,7 +335,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;
@@ -383,8 +357,7 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) con
 	return gf;
 }
 
-
-void btRigidBody::integrateVelocities(btScalar step) 
+void btRigidBody::integrateVelocities(btScalar step)
 {
 	if (isStaticOrKinematicObject())
 		return;
@@ -393,30 +366,28 @@ 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;
 	}
-
 }
 
 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;
 	}
@@ -426,10 +397,6 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
 	updateInertiaTensor();
 }
 
-
-
-
-
 void btRigidBody::addConstraintRef(btTypedConstraint* c)
 {
 	///disable collision with the 'other' body
@@ -450,39 +417,39 @@ void btRigidBody::addConstraintRef(btTypedConstraint* c)
 		{
 			colObjB->setIgnoreCollisionCheck(colObjA, true);
 		}
-	} 
+	}
 }
 
 void btRigidBody::removeConstraintRef(btTypedConstraint* c)
 {
 	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);
-        }
-    }
+	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);
 
@@ -504,7 +471,7 @@ 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.
@@ -515,13 +482,9 @@ const char*	btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali
 	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/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h
index 372245031b..05f270a4b8 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h
@@ -25,209 +25,195 @@ 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;
-	
+	int m_rigidbodyFlags;
 
-protected:
-
-	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_spinningFriction;//torsional friction around contact normal
-        
+		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_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))
+		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 setGravity(const btVector3& acceleration);
 
-	const btVector3&	getGravity() const
+	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
 	{
@@ -249,18 +235,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;
@@ -268,20 +256,21 @@ 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; }
+	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
@@ -293,90 +282,93 @@ 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;
 	}
-	
-	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;
 	}
-	
-  	void applyTorqueImpulse(const btVector3& torque)
+
+	void applyTorqueImpulse(const btVector3& torque)
 	{
-			m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
+		m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
 	}
-	
-	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 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;
 	}
 
-	inline void setAngularVelocity(const btVector3& ang_vel) 
-	{ 
+	inline void setAngularVelocity(const btVector3& ang_vel)
+	{
 		m_updateRevision++;
-		m_angularVelocity = ang_vel; 
+		m_angularVelocity = ang_vel;
 	}
 
 	btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const
@@ -388,18 +380,13 @@ public:
 		//		return 	(m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
 	}
 
-	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();
@@ -409,7 +396,6 @@ public:
 		btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0);
 
 		return m_inverseMass + normal.dot(vec);
-
 	}
 
 	SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const
@@ -418,26 +404,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;
 
@@ -445,41 +430,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)
@@ -487,27 +470,27 @@ 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);
 	}
@@ -525,7 +508,7 @@ public:
 		return m_constraintRefs.size();
 	}
 
-	void	setFlags(int flags)
+	void setFlags(int flags)
 	{
 		m_rigidbodyFlags = flags;
 	}
@@ -535,12 +518,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;
 
@@ -550,70 +530,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/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
index 6f63b87c80..8103390fb1 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
+++ b/thirdparty/bullet/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, int group, int 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/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
index 44b7e7fb34..12be231c7f 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
+++ b/thirdparty/bullet/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, int group, int 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/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp
index fc54f0ba6e..17287aa82a 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "LinearMath/btScalar.h"
 #include "LinearMath/btThreads.h"
 #include "btSimulationIslandManagerMt.h"
@@ -27,273 +26,259 @@ subject to the following restrictions:
 //#include <stdio.h>
 #include "LinearMath/btQuickprof.h"
 
-
-SIMD_FORCE_INLINE int calcBatchCost( int bodies, int manifolds, int constraints )
+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;
+	// 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 )
+SIMD_FORCE_INLINE int calcBatchCost(const btSimulationIslandManagerMt::Island* island)
 {
-    return calcBatchCost( island->bodyArray.size(), island->manifoldArray.size(), island->constraintArray.size() );
+	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;
+	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 );
+	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)
+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();
+	int islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag();
 	return islandId;
 }
 
-
-SIMD_FORCE_INLINE	int	btGetConstraintIslandId( const btTypedConstraint* lhs )
+SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs)
 {
-    const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
-    const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
-    int islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
-    return islandId;
+	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;
-    }
+	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();
-    }
+	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 )
+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 ] );
-    }
+	// 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 )
+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;
+	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 );
-    }
-}
+	// 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 )
+btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland(int id)
 {
-    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;
+	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 )
+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;
-}
+	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 )
+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 endIslandIndex = 1;
 	int startIslandIndex;
 
 	//update the sleeping state for bodies, if all are sleeping
-	for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
+	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++)
+		for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++)
 		{
 		}
 
@@ -302,21 +287,21 @@ void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btColl
 		bool allSleeping = true;
 
 		int idx;
-		for (idx=startIslandIndex;idx<endIslandIndex;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");
+				//				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)
+				if (colObj0->getActivationState() == ACTIVE_TAG ||
+					colObj0->getActivationState() == DISABLE_DEACTIVATION)
 				{
 					allSleeping = false;
 					break;
@@ -327,43 +312,43 @@ void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btColl
 		if (allSleeping)
 		{
 			int idx;
-			for (idx=startIslandIndex;idx<endIslandIndex;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");
+					//					printf("error in island management\n");
 				}
 
 				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 
 				if (colObj0->getIslandTag() == islandId)
 				{
-					colObj0->setActivationState( ISLAND_SLEEPING );
+					colObj0->setActivationState(ISLAND_SLEEPING);
 				}
 			}
-		} else
+		}
+		else
 		{
-
 			int idx;
-			for (idx=startIslandIndex;idx<endIslandIndex;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");
+					//					printf("error in island management\n");
 				}
 
 				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 
 				if (colObj0->getIslandTag() == islandId)
 				{
-					if ( colObj0->getActivationState() == ISLAND_SLEEPING)
+					if (colObj0->getActivationState() == ISLAND_SLEEPING)
 					{
-						colObj0->setActivationState( WANTS_DEACTIVATION);
+						colObj0->setActivationState(WANTS_DEACTIVATION);
 						colObj0->setDeactivationTime(0.f);
 					}
 				}
@@ -372,352 +357,338 @@ void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btColl
 	}
 }
 
-
-void btSimulationIslandManagerMt::addBodiesToIslands( btCollisionWorld* collisionWorld )
+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 );
-            }
-        }
-    }
+	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;
 
-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 );
-                }
-            }
-        }
-    }
+			// 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::addConstraintsToIslands( btAlignedObjectArray<btTypedConstraint*>& constraints )
+void btSimulationIslandManagerMt::addManifoldsToIslands(btDispatcher* dispatcher)
 {
-    // walk constraints
-    for ( int i = 0; i < constraints.size(); i++ )
-    {
-        // scatter constraints into various islands
-        btTypedConstraint* constraint = constraints[ i ];
-        if ( constraint->isEnabled() )
-        {
-            int islandId = btGetConstraintIslandId( constraint );
-            // if island is not sleeping,
-            if ( Island* island = getIsland( islandId ) )
-            {
-                island->constraintArray.push_back( constraint );
-            }
-        }
-    }
+	// 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 = btGetConstraintIslandId(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++;
-    }
-}
+	// 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
-    );
+	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 )
+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);
-    }
+	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 );
-        }
-    }
-};
+	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 )
+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 );
+	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
-                                                        )
+void btSimulationIslandManagerMt::buildAndProcessIslands(btDispatcher* dispatcher,
+														 btCollisionWorld* collisionWorld,
+														 btAlignedObjectArray<btTypedConstraint*>& constraints,
+														 const SolverParams& solverParams)
 {
 	BT_PROFILE("buildAndProcessIslands");
 	btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
 
-	buildIslands(dispatcher,collisionWorld);
+	buildIslands(dispatcher, collisionWorld);
 
-	if(!getSplitIslands())
+	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
-                           );
+		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 );
+		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/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h
index 563577a6f4..ab73a899f1 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h
@@ -35,80 +35,78 @@ class btIDebugDraw;
 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;
+	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);
+		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);
 
-    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;
+	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();
 
-    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 buildAndProcessIslands(btDispatcher* dispatcher,
+										btCollisionWorld* collisionWorld,
+										btAlignedObjectArray<btTypedConstraint*>& constraints,
+										const SolverParams& solverParams);
 
-	virtual void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
+	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;
-    }
+	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
-
+#endif  //BT_SIMULATION_ISLAND_MANAGER_H