Merge pull request #24740 from OBKF/update-bullet-physics

Update Bullet physics to commit 126b676

1 files changed, 1244 insertions, 1311 deletions

diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp
index 4306c37e49..2718da4a50 100644
--- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp
+++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "btSequentialImpulseConstraintSolverMt.h"
 
 #include "LinearMath/btQuickprof.h"
@@ -23,8 +22,6 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 
-
-
 bool btSequentialImpulseConstraintSolverMt::s_allowNestedParallelForLoops = false;  // some task schedulers don't like nested loops
 int btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching = 250;
 int btSequentialImpulseConstraintSolverMt::s_minBatchSize = 50;
@@ -32,613 +29,594 @@ int btSequentialImpulseConstraintSolverMt::s_maxBatchSize = 100;
 btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_contactBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D;
 btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_jointBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D;
 
-
 btSequentialImpulseConstraintSolverMt::btSequentialImpulseConstraintSolverMt()
 {
-    m_numFrictionDirections = 1;
-    m_useBatching = false;
-    m_useObsoleteJointConstraints = false;
+	m_numFrictionDirections = 1;
+	m_useBatching = false;
+	m_useObsoleteJointConstraints = false;
 }
 
-
 btSequentialImpulseConstraintSolverMt::~btSequentialImpulseConstraintSolverMt()
 {
 }
 
-
 void btSequentialImpulseConstraintSolverMt::setupBatchedContactConstraints()
 {
-    BT_PROFILE("setupBatchedContactConstraints");
-    m_batchedContactConstraints.setup( &m_tmpSolverContactConstraintPool,
-        m_tmpSolverBodyPool,
-        s_contactBatchingMethod,
-        s_minBatchSize,
-        s_maxBatchSize,
-        &m_scratchMemory
-    );
+	BT_PROFILE("setupBatchedContactConstraints");
+	m_batchedContactConstraints.setup(&m_tmpSolverContactConstraintPool,
+									  m_tmpSolverBodyPool,
+									  s_contactBatchingMethod,
+									  s_minBatchSize,
+									  s_maxBatchSize,
+									  &m_scratchMemory);
 }
 
-
 void btSequentialImpulseConstraintSolverMt::setupBatchedJointConstraints()
 {
-    BT_PROFILE("setupBatchedJointConstraints");
-    m_batchedJointConstraints.setup( &m_tmpSolverNonContactConstraintPool,
-        m_tmpSolverBodyPool,
-        s_jointBatchingMethod,
-        s_minBatchSize,
-        s_maxBatchSize,
-        &m_scratchMemory
-    );
+	BT_PROFILE("setupBatchedJointConstraints");
+	m_batchedJointConstraints.setup(&m_tmpSolverNonContactConstraintPool,
+									m_tmpSolverBodyPool,
+									s_jointBatchingMethod,
+									s_minBatchSize,
+									s_maxBatchSize,
+									&m_scratchMemory);
 }
 
-
 void btSequentialImpulseConstraintSolverMt::internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal)
 {
-    btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[iContactConstraint];
-
-    btVector3 rel_pos1;
-    btVector3 rel_pos2;
-    btScalar relaxation;
-
-    int solverBodyIdA = contactConstraint.m_solverBodyIdA;
-    int solverBodyIdB = contactConstraint.m_solverBodyIdB;
-
-    btSolverBody* solverBodyA = &m_tmpSolverBodyPool[ solverBodyIdA ];
-    btSolverBody* solverBodyB = &m_tmpSolverBodyPool[ solverBodyIdB ];
-
-    btRigidBody* colObj0 = solverBodyA->m_originalBody;
-    btRigidBody* colObj1 = solverBodyB->m_originalBody;
-
-    btManifoldPoint& cp = *static_cast<btManifoldPoint*>( contactConstraint.m_originalContactPoint );
-
-    const btVector3& pos1 = cp.getPositionWorldOnA();
-    const btVector3& pos2 = cp.getPositionWorldOnB();
-
-    rel_pos1 = pos1 - solverBodyA->getWorldTransform().getOrigin();
-    rel_pos2 = pos2 - solverBodyB->getWorldTransform().getOrigin();
-
-    btVector3 vel1;
-    btVector3 vel2;
-
-    solverBodyA->getVelocityInLocalPointNoDelta( rel_pos1, vel1 );
-    solverBodyB->getVelocityInLocalPointNoDelta( rel_pos2, vel2 );
-
-    btVector3 vel = vel1 - vel2;
-    btScalar rel_vel = cp.m_normalWorldOnB.dot( vel );
-
-    setupContactConstraint( contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2 );
-
-    // setup rolling friction constraints
-    int rollingFrictionIndex = m_rollingFrictionIndexTable[iContactConstraint];
-    if (rollingFrictionIndex >= 0)
-    {
-        btSolverConstraint& spinningFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ rollingFrictionIndex ];
-        btAssert( spinningFrictionConstraint.m_frictionIndex == iContactConstraint );
-        setupTorsionalFrictionConstraint( spinningFrictionConstraint,
-            cp.m_normalWorldOnB,
-            solverBodyIdA,
-            solverBodyIdB,
-            cp,
-            cp.m_combinedSpinningFriction,
-            rel_pos1,
-            rel_pos2,
-            colObj0,
-            colObj1,
-            relaxation,
-            0.0f,
-            0.0f
-        );
-        btVector3 axis[2];
-        btPlaneSpace1( cp.m_normalWorldOnB, axis[0], axis[1] );
-        axis[0].normalize();
-        axis[1].normalize();
-
-        applyAnisotropicFriction( colObj0, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION );
-        applyAnisotropicFriction( colObj1, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION );
-        applyAnisotropicFriction( colObj0, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION );
-        applyAnisotropicFriction( colObj1, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION );
-        // put the largest axis first
-        if (axis[1].length2() > axis[0].length2())
-        {
-            btSwap(axis[0], axis[1]);
-        }
-        const btScalar kRollingFrictionThreshold = 0.001f;
-        for (int i = 0; i < 2; ++i)
-        {
-            int iRollingFric = rollingFrictionIndex + 1 + i;
-            btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ iRollingFric ];
-            btAssert(rollingFrictionConstraint.m_frictionIndex == iContactConstraint);
-            btVector3 dir = axis[i];
-            if ( dir.length() > kRollingFrictionThreshold )
-            {
-                setupTorsionalFrictionConstraint( rollingFrictionConstraint,
-                    dir,
-                    solverBodyIdA,
-                    solverBodyIdB,
-                    cp,
-                    cp.m_combinedRollingFriction,
-                    rel_pos1,
-                    rel_pos2,
-                    colObj0,
-                    colObj1,
-                    relaxation,
-                    0.0f,
-                    0.0f
-                );
-            }
-            else
-            {
-                rollingFrictionConstraint.m_frictionIndex = -1;  // disable constraint
-            }
-        }
-    }
-
-    // setup friction constraints
-    //	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
-    {
-        ///Bullet has several options to set the friction directions
-        ///By default, each contact has only a single friction direction that is recomputed automatically very frame
-        ///based on the relative linear velocity.
-        ///If the relative velocity it zero, it will automatically compute a friction direction.
-
-        ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
-        ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
-        ///
-        ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
-        ///
-        ///The user can manually override the friction directions for certain contacts using a contact callback,
-        ///and set the cp.m_lateralFrictionInitialized to true
-        ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
-        ///this will give a conveyor belt effect
-        ///
-	    btSolverConstraint* frictionConstraint1 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex];
-        btAssert(frictionConstraint1->m_frictionIndex == iContactConstraint);
-
-        btSolverConstraint* frictionConstraint2 = NULL;
-        if ( infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS )
-        {
-            frictionConstraint2 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex + 1];
-            btAssert( frictionConstraint2->m_frictionIndex == iContactConstraint );
-        }
-
-        if ( !( infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING ) || !( cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED ) )
-        {
-            cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
-            btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
-            if ( !( infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION ) && lat_rel_vel > SIMD_EPSILON )
-            {
-                cp.m_lateralFrictionDir1 *= 1.f / btSqrt( lat_rel_vel );
-                applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION );
-                applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION );
-                setupFrictionConstraint( *frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal );
-
-                if ( frictionConstraint2 )
-                {
-                    cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross( cp.m_normalWorldOnB );
-                    cp.m_lateralFrictionDir2.normalize();//??
-                    applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION );
-                    applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION );
-                    setupFrictionConstraint( *frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal );
-                }
-            }
-            else
-            {
-                btPlaneSpace1( cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2 );
-
-                applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION );
-                applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION );
-                setupFrictionConstraint( *frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal );
-
-                if ( frictionConstraint2 )
-                {
-                    applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION );
-                    applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION );
-                    setupFrictionConstraint( *frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal );
-                }
-
-                if ( ( infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) && ( infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION ) )
-                {
-                    cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
-                }
-            }
-        }
-        else
-        {
-            setupFrictionConstraint( *frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM );
-            if ( frictionConstraint2 )
-            {
-                setupFrictionConstraint( *frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM );
-            }
-        }
-    }
-
-    setFrictionConstraintImpulse( contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal );
-}
+	btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[iContactConstraint];
+
+	btVector3 rel_pos1;
+	btVector3 rel_pos2;
+	btScalar relaxation;
+
+	int solverBodyIdA = contactConstraint.m_solverBodyIdA;
+	int solverBodyIdB = contactConstraint.m_solverBodyIdB;
+
+	btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
+	btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
+
+	btRigidBody* colObj0 = solverBodyA->m_originalBody;
+	btRigidBody* colObj1 = solverBodyB->m_originalBody;
+
+	btManifoldPoint& cp = *static_cast<btManifoldPoint*>(contactConstraint.m_originalContactPoint);
+
+	const btVector3& pos1 = cp.getPositionWorldOnA();
+	const btVector3& pos2 = cp.getPositionWorldOnB();
+
+	rel_pos1 = pos1 - solverBodyA->getWorldTransform().getOrigin();
+	rel_pos2 = pos2 - solverBodyB->getWorldTransform().getOrigin();
+
+	btVector3 vel1;
+	btVector3 vel2;
+
+	solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1, vel1);
+	solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2, vel2);
+
+	btVector3 vel = vel1 - vel2;
+	btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
+
+	setupContactConstraint(contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
+
+	// setup rolling friction constraints
+	int rollingFrictionIndex = m_rollingFrictionIndexTable[iContactConstraint];
+	if (rollingFrictionIndex >= 0)
+	{
+		btSolverConstraint& spinningFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[rollingFrictionIndex];
+		btAssert(spinningFrictionConstraint.m_frictionIndex == iContactConstraint);
+		setupTorsionalFrictionConstraint(spinningFrictionConstraint,
+										 cp.m_normalWorldOnB,
+										 solverBodyIdA,
+										 solverBodyIdB,
+										 cp,
+										 cp.m_combinedSpinningFriction,
+										 rel_pos1,
+										 rel_pos2,
+										 colObj0,
+										 colObj1,
+										 relaxation,
+										 0.0f,
+										 0.0f);
+		btVector3 axis[2];
+		btPlaneSpace1(cp.m_normalWorldOnB, axis[0], axis[1]);
+		axis[0].normalize();
+		axis[1].normalize();
+
+		applyAnisotropicFriction(colObj0, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+		applyAnisotropicFriction(colObj1, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+		applyAnisotropicFriction(colObj0, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+		applyAnisotropicFriction(colObj1, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
+		// put the largest axis first
+		if (axis[1].length2() > axis[0].length2())
+		{
+			btSwap(axis[0], axis[1]);
+		}
+		const btScalar kRollingFrictionThreshold = 0.001f;
+		for (int i = 0; i < 2; ++i)
+		{
+			int iRollingFric = rollingFrictionIndex + 1 + i;
+			btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric];
+			btAssert(rollingFrictionConstraint.m_frictionIndex == iContactConstraint);
+			btVector3 dir = axis[i];
+			if (dir.length() > kRollingFrictionThreshold)
+			{
+				setupTorsionalFrictionConstraint(rollingFrictionConstraint,
+												 dir,
+												 solverBodyIdA,
+												 solverBodyIdB,
+												 cp,
+												 cp.m_combinedRollingFriction,
+												 rel_pos1,
+												 rel_pos2,
+												 colObj0,
+												 colObj1,
+												 relaxation,
+												 0.0f,
+												 0.0f);
+			}
+			else
+			{
+				rollingFrictionConstraint.m_frictionIndex = -1;  // disable constraint
+			}
+		}
+	}
+
+	// setup friction constraints
+	//	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
+	{
+		///Bullet has several options to set the friction directions
+		///By default, each contact has only a single friction direction that is recomputed automatically very frame
+		///based on the relative linear velocity.
+		///If the relative velocity it zero, it will automatically compute a friction direction.
+
+		///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
+		///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
+		///
+		///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
+		///
+		///The user can manually override the friction directions for certain contacts using a contact callback,
+		///and set the cp.m_lateralFrictionInitialized to true
+		///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
+		///this will give a conveyor belt effect
+		///
+		btSolverConstraint* frictionConstraint1 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex];
+		btAssert(frictionConstraint1->m_frictionIndex == iContactConstraint);
+
+		btSolverConstraint* frictionConstraint2 = NULL;
+		if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
+		{
+			frictionConstraint2 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex + 1];
+			btAssert(frictionConstraint2->m_frictionIndex == iContactConstraint);
+		}
+
+		if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
+		{
+			cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+			btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+			if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
+			{
+				cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel);
+				applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+				applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+				setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
+
+				if (frictionConstraint2)
+				{
+					cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
+					cp.m_lateralFrictionDir2.normalize();  //??
+					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
+				}
+			}
+			else
+			{
+				btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2);
+
+				applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+				applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+				setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
+
+				if (frictionConstraint2)
+				{
+					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
+				}
+
+				if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
+				{
+					cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
+				}
+			}
+		}
+		else
+		{
+			setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
+			if (frictionConstraint2)
+			{
+				setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
+			}
+		}
+	}
 
+	setFrictionConstraintImpulse(contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
+}
 
 struct SetupContactConstraintsLoop : public btIParallelForBody
 {
-    btSequentialImpulseConstraintSolverMt* m_solver;
-    const btBatchedConstraints* m_bc;
-    const btContactSolverInfo* m_infoGlobal;
-
-    SetupContactConstraintsLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, const btContactSolverInfo& infoGlobal )
-    {
-        m_solver = solver;
-        m_bc = bc;
-        m_infoGlobal = &infoGlobal;
-    }
-    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
-    {
-        BT_PROFILE( "SetupContactConstraintsLoop" );
-        for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch )
-        {
-            const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ];
-            for (int i = batch.begin; i < batch.end; ++i)
-            {
-                int iContact = m_bc->m_constraintIndices[i];
-                m_solver->internalSetupContactConstraints( iContact, *m_infoGlobal );
-            }
-        }
-    }
-};
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
+	const btContactSolverInfo* m_infoGlobal;
 
+	SetupContactConstraintsLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, const btContactSolverInfo& infoGlobal)
+	{
+		m_solver = solver;
+		m_bc = bc;
+		m_infoGlobal = &infoGlobal;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("SetupContactConstraintsLoop");
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			for (int i = batch.begin; i < batch.end; ++i)
+			{
+				int iContact = m_bc->m_constraintIndices[i];
+				m_solver->internalSetupContactConstraints(iContact, *m_infoGlobal);
+			}
+		}
+	}
+};
 
 void btSequentialImpulseConstraintSolverMt::setupAllContactConstraints(const btContactSolverInfo& infoGlobal)
 {
-    BT_PROFILE( "setupAllContactConstraints" );
-    if ( m_useBatching )
-    {
-        const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
-        SetupContactConstraintsLoop loop( this, &batchedCons, infoGlobal );
-        for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase )
-        {
-            int iPhase = batchedCons.m_phaseOrder[ iiPhase ];
-            const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ];
-            int grainSize = 1;
-            btParallelFor( phase.begin, phase.end, grainSize, loop );
-        }
-    }
-    else
-    {
-        for ( int i = 0; i < m_tmpSolverContactConstraintPool.size(); ++i )
-        {
-            internalSetupContactConstraints( i, infoGlobal );
-        }
-    }
+	BT_PROFILE("setupAllContactConstraints");
+	if (m_useBatching)
+	{
+		const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+		SetupContactConstraintsLoop loop(this, &batchedCons, infoGlobal);
+		for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+		{
+			int iPhase = batchedCons.m_phaseOrder[iiPhase];
+			const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+			int grainSize = 1;
+			btParallelFor(phase.begin, phase.end, grainSize, loop);
+		}
+	}
+	else
+	{
+		for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); ++i)
+		{
+			internalSetupContactConstraints(i, infoGlobal);
+		}
+	}
 }
 
-
-int	btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btCollisionObject& body,btScalar timeStep)
+int btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btCollisionObject& body, btScalar timeStep)
 {
-    //
-    // getOrInitSolverBody is threadsafe only for a single thread per solver (with potentially multiple solvers)
-    //
-    // getOrInitSolverBodyThreadsafe -- attempts to be fully threadsafe (however may affect determinism)
-    //
-    int solverBodyId = -1;
-    bool isRigidBodyType = btRigidBody::upcast( &body ) != NULL;
-    if ( isRigidBodyType && !body.isStaticOrKinematicObject() )
-    {
-        // dynamic body
-        // Dynamic bodies can only be in one island, so it's safe to write to the companionId
-        solverBodyId = body.getCompanionId();
-        if ( solverBodyId < 0 )
-        {
-            m_bodySolverArrayMutex.lock();
-            // now that we have the lock, check again
-            solverBodyId = body.getCompanionId();
-            if ( solverBodyId < 0 )
-            {
-                solverBodyId = m_tmpSolverBodyPool.size();
-                btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-                initSolverBody( &solverBody, &body, timeStep );
-                body.setCompanionId( solverBodyId );
-            }
-            m_bodySolverArrayMutex.unlock();
-        }
-    }
-    else if (isRigidBodyType && body.isKinematicObject())
-    {
-        //
-        // NOTE: must test for kinematic before static because some kinematic objects also
-        //   identify as "static"
-        //
-        // Kinematic bodies can be in multiple islands at once, so it is a
-        // race condition to write to them, so we use an alternate method
-        // to record the solverBodyId
-        int uniqueId = body.getWorldArrayIndex();
-        const int INVALID_SOLVER_BODY_ID = -1;
-        if (m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId )
-        {
-            m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock();
-            // now that we have the lock, check again
-            if ( m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId )
-            {
-                m_kinematicBodyUniqueIdToSolverBodyTable.resize( uniqueId + 1, INVALID_SOLVER_BODY_ID );
-            }
-            m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock();
-        }
-        solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ];
-        // if no table entry yet,
-        if ( INVALID_SOLVER_BODY_ID == solverBodyId )
-        {
-            // need to acquire both locks
-            m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock();
-            m_bodySolverArrayMutex.lock();
-            // now that we have the lock, check again
-            solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ];
-            if ( INVALID_SOLVER_BODY_ID == solverBodyId )
-            {
-                // create a table entry for this body
-                solverBodyId = m_tmpSolverBodyPool.size();
-                btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-                initSolverBody( &solverBody, &body, timeStep );
-                m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ] = solverBodyId;
-            }
-            m_bodySolverArrayMutex.unlock();
-            m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock();
-        }
-    }
-    else
-    {
-        // all fixed bodies (inf mass) get mapped to a single solver id
-        if ( m_fixedBodyId < 0 )
-        {
-            m_bodySolverArrayMutex.lock();
-            // now that we have the lock, check again
-            if ( m_fixedBodyId < 0 )
-            {
-                m_fixedBodyId = m_tmpSolverBodyPool.size();
-                btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
-                initSolverBody( &fixedBody, 0, timeStep );
-            }
-            m_bodySolverArrayMutex.unlock();
-        }
-        solverBodyId = m_fixedBodyId;
-    }
-    btAssert( solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size() );
+	//
+	// getOrInitSolverBody is threadsafe only for a single thread per solver (with potentially multiple solvers)
+	//
+	// getOrInitSolverBodyThreadsafe -- attempts to be fully threadsafe (however may affect determinism)
+	//
+	int solverBodyId = -1;
+	bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
+	if (isRigidBodyType && !body.isStaticOrKinematicObject())
+	{
+		// dynamic body
+		// Dynamic bodies can only be in one island, so it's safe to write to the companionId
+		solverBodyId = body.getCompanionId();
+		if (solverBodyId < 0)
+		{
+			m_bodySolverArrayMutex.lock();
+			// now that we have the lock, check again
+			solverBodyId = body.getCompanionId();
+			if (solverBodyId < 0)
+			{
+				solverBodyId = m_tmpSolverBodyPool.size();
+				btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+				initSolverBody(&solverBody, &body, timeStep);
+				body.setCompanionId(solverBodyId);
+			}
+			m_bodySolverArrayMutex.unlock();
+		}
+	}
+	else if (isRigidBodyType && body.isKinematicObject())
+	{
+		//
+		// NOTE: must test for kinematic before static because some kinematic objects also
+		//   identify as "static"
+		//
+		// Kinematic bodies can be in multiple islands at once, so it is a
+		// race condition to write to them, so we use an alternate method
+		// to record the solverBodyId
+		int uniqueId = body.getWorldArrayIndex();
+		const int INVALID_SOLVER_BODY_ID = -1;
+		if (m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId)
+		{
+			m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock();
+			// now that we have the lock, check again
+			if (m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId)
+			{
+				m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
+			}
+			m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock();
+		}
+		solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId];
+		// if no table entry yet,
+		if (INVALID_SOLVER_BODY_ID == solverBodyId)
+		{
+			// need to acquire both locks
+			m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock();
+			m_bodySolverArrayMutex.lock();
+			// now that we have the lock, check again
+			solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId];
+			if (INVALID_SOLVER_BODY_ID == solverBodyId)
+			{
+				// create a table entry for this body
+				solverBodyId = m_tmpSolverBodyPool.size();
+				btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+				initSolverBody(&solverBody, &body, timeStep);
+				m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId] = solverBodyId;
+			}
+			m_bodySolverArrayMutex.unlock();
+			m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock();
+		}
+	}
+	else
+	{
+		// all fixed bodies (inf mass) get mapped to a single solver id
+		if (m_fixedBodyId < 0)
+		{
+			m_bodySolverArrayMutex.lock();
+			// now that we have the lock, check again
+			if (m_fixedBodyId < 0)
+			{
+				m_fixedBodyId = m_tmpSolverBodyPool.size();
+				btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+				initSolverBody(&fixedBody, 0, timeStep);
+			}
+			m_bodySolverArrayMutex.unlock();
+		}
+		solverBodyId = m_fixedBodyId;
+	}
+	btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size());
 	return solverBodyId;
 }
 
-
 void btSequentialImpulseConstraintSolverMt::internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
 {
-    BT_PROFILE("internalCollectContactManifoldCachedInfo");
-    for (int i = 0; i < numManifolds; ++i)
-    {
-        btContactManifoldCachedInfo* cachedInfo = &cachedInfoArray[i];
-        btPersistentManifold* manifold = manifoldPtr[i];
-        btCollisionObject* colObj0 = (btCollisionObject*) manifold->getBody0();
-        btCollisionObject* colObj1 = (btCollisionObject*) manifold->getBody1();
-
-        int solverBodyIdA = getOrInitSolverBodyThreadsafe( *colObj0, infoGlobal.m_timeStep );
-        int solverBodyIdB = getOrInitSolverBodyThreadsafe( *colObj1, infoGlobal.m_timeStep );
-
-        cachedInfo->solverBodyIds[ 0 ] = solverBodyIdA;
-        cachedInfo->solverBodyIds[ 1 ] = solverBodyIdB;
-        cachedInfo->numTouchingContacts = 0;
-
-        btSolverBody* solverBodyA = &m_tmpSolverBodyPool[ solverBodyIdA ];
-        btSolverBody* solverBodyB = &m_tmpSolverBodyPool[ solverBodyIdB ];
-
-        // A contact manifold between 2 static object should not exist!
-        // check the collision flags of your objects if this assert fires.
-        // Incorrectly set collision object flags can degrade performance in various ways.
-        btAssert( !m_tmpSolverBodyPool[ solverBodyIdA ].m_invMass.isZero() || !m_tmpSolverBodyPool[ solverBodyIdB ].m_invMass.isZero() );
-
-        int iContact = 0;
-        for ( int j = 0; j < manifold->getNumContacts(); j++ )
-        {
-            btManifoldPoint& cp = manifold->getContactPoint( j );
-
-            if ( cp.getDistance() <= manifold->getContactProcessingThreshold() )
-            {
-                cachedInfo->contactPoints[ iContact ] = &cp;
-                cachedInfo->contactHasRollingFriction[ iContact ] = ( cp.m_combinedRollingFriction > 0.f );
-                iContact++;
-            }
-        }
-        cachedInfo->numTouchingContacts = iContact;
-    }
-}
+	BT_PROFILE("internalCollectContactManifoldCachedInfo");
+	for (int i = 0; i < numManifolds; ++i)
+	{
+		btContactManifoldCachedInfo* cachedInfo = &cachedInfoArray[i];
+		btPersistentManifold* manifold = manifoldPtr[i];
+		btCollisionObject* colObj0 = (btCollisionObject*)manifold->getBody0();
+		btCollisionObject* colObj1 = (btCollisionObject*)manifold->getBody1();
+
+		int solverBodyIdA = getOrInitSolverBodyThreadsafe(*colObj0, infoGlobal.m_timeStep);
+		int solverBodyIdB = getOrInitSolverBodyThreadsafe(*colObj1, infoGlobal.m_timeStep);
+
+		cachedInfo->solverBodyIds[0] = solverBodyIdA;
+		cachedInfo->solverBodyIds[1] = solverBodyIdB;
+		cachedInfo->numTouchingContacts = 0;
+
+		btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
+		btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
 
+		// A contact manifold between 2 static object should not exist!
+		// check the collision flags of your objects if this assert fires.
+		// Incorrectly set collision object flags can degrade performance in various ways.
+		btAssert(!m_tmpSolverBodyPool[solverBodyIdA].m_invMass.isZero() || !m_tmpSolverBodyPool[solverBodyIdB].m_invMass.isZero());
+
+		int iContact = 0;
+		for (int j = 0; j < manifold->getNumContacts(); j++)
+		{
+			btManifoldPoint& cp = manifold->getContactPoint(j);
+
+			if (cp.getDistance() <= manifold->getContactProcessingThreshold())
+			{
+				cachedInfo->contactPoints[iContact] = &cp;
+				cachedInfo->contactHasRollingFriction[iContact] = (cp.m_combinedRollingFriction > 0.f);
+				iContact++;
+			}
+		}
+		cachedInfo->numTouchingContacts = iContact;
+	}
+}
 
 struct CollectContactManifoldCachedInfoLoop : public btIParallelForBody
 {
-    btSequentialImpulseConstraintSolverMt* m_solver;
-    btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray;
-    btPersistentManifold** m_manifoldPtr;
-    const btContactSolverInfo* m_infoGlobal;
-
-    CollectContactManifoldCachedInfoLoop( btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, const btContactSolverInfo& infoGlobal )
-    {
-        m_solver = solver;
-        m_cachedInfoArray = cachedInfoArray;
-        m_manifoldPtr = manifoldPtr;
-        m_infoGlobal = &infoGlobal;
-    }
-    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
-    {
-        m_solver->internalCollectContactManifoldCachedInfo( m_cachedInfoArray + iBegin, m_manifoldPtr + iBegin, iEnd - iBegin, *m_infoGlobal );
-    }
-};
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray;
+	btPersistentManifold** m_manifoldPtr;
+	const btContactSolverInfo* m_infoGlobal;
 
+	CollectContactManifoldCachedInfoLoop(btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, const btContactSolverInfo& infoGlobal)
+	{
+		m_solver = solver;
+		m_cachedInfoArray = cachedInfoArray;
+		m_manifoldPtr = manifoldPtr;
+		m_infoGlobal = &infoGlobal;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalCollectContactManifoldCachedInfo(m_cachedInfoArray + iBegin, m_manifoldPtr + iBegin, iEnd - iBegin, *m_infoGlobal);
+	}
+};
 
 void btSequentialImpulseConstraintSolverMt::internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds)
 {
-    BT_PROFILE("internalAllocContactConstraints");
-    // possibly parallel part
-    for ( int iManifold = 0; iManifold < numManifolds; ++iManifold )
-    {
-        const btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[ iManifold ];
-        int contactIndex = cachedInfo.contactIndex;
-        int frictionIndex = contactIndex * m_numFrictionDirections;
-        int rollingFrictionIndex = cachedInfo.rollingFrictionIndex;
-        for ( int i = 0; i < cachedInfo.numTouchingContacts; i++ )
-        {
-            btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[contactIndex];
-            contactConstraint.m_solverBodyIdA = cachedInfo.solverBodyIds[ 0 ];
-            contactConstraint.m_solverBodyIdB = cachedInfo.solverBodyIds[ 1 ];
-            contactConstraint.m_originalContactPoint = cachedInfo.contactPoints[ i ];
-
-            // allocate the friction constraints
-            contactConstraint.m_frictionIndex = frictionIndex;
-            for ( int iDir = 0; iDir < m_numFrictionDirections; ++iDir )
-            {
-                btSolverConstraint& frictionConstraint = m_tmpSolverContactFrictionConstraintPool[frictionIndex];
-                frictionConstraint.m_frictionIndex = contactIndex;
-                frictionIndex++;
-            }
-
-            // allocate rolling friction constraints
-            if ( cachedInfo.contactHasRollingFriction[ i ] )
-            {
-                m_rollingFrictionIndexTable[ contactIndex ] = rollingFrictionIndex;
-                // allocate 3 (although we may use only 2 sometimes)
-                for ( int i = 0; i < 3; i++ )
-                {
-                    m_tmpSolverContactRollingFrictionConstraintPool[ rollingFrictionIndex ].m_frictionIndex = contactIndex;
-                    rollingFrictionIndex++;
-                }
-            }
-            else
-            {
-                // indicate there is no rolling friction for this contact point
-                m_rollingFrictionIndexTable[ contactIndex ] = -1;
-            }
-            contactIndex++;
-        }
-    }
-}
+	BT_PROFILE("internalAllocContactConstraints");
+	// possibly parallel part
+	for (int iManifold = 0; iManifold < numManifolds; ++iManifold)
+	{
+		const btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[iManifold];
+		int contactIndex = cachedInfo.contactIndex;
+		int frictionIndex = contactIndex * m_numFrictionDirections;
+		int rollingFrictionIndex = cachedInfo.rollingFrictionIndex;
+		for (int i = 0; i < cachedInfo.numTouchingContacts; i++)
+		{
+			btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[contactIndex];
+			contactConstraint.m_solverBodyIdA = cachedInfo.solverBodyIds[0];
+			contactConstraint.m_solverBodyIdB = cachedInfo.solverBodyIds[1];
+			contactConstraint.m_originalContactPoint = cachedInfo.contactPoints[i];
+
+			// allocate the friction constraints
+			contactConstraint.m_frictionIndex = frictionIndex;
+			for (int iDir = 0; iDir < m_numFrictionDirections; ++iDir)
+			{
+				btSolverConstraint& frictionConstraint = m_tmpSolverContactFrictionConstraintPool[frictionIndex];
+				frictionConstraint.m_frictionIndex = contactIndex;
+				frictionIndex++;
+			}
 
+			// allocate rolling friction constraints
+			if (cachedInfo.contactHasRollingFriction[i])
+			{
+				m_rollingFrictionIndexTable[contactIndex] = rollingFrictionIndex;
+				// allocate 3 (although we may use only 2 sometimes)
+				for (int i = 0; i < 3; i++)
+				{
+					m_tmpSolverContactRollingFrictionConstraintPool[rollingFrictionIndex].m_frictionIndex = contactIndex;
+					rollingFrictionIndex++;
+				}
+			}
+			else
+			{
+				// indicate there is no rolling friction for this contact point
+				m_rollingFrictionIndexTable[contactIndex] = -1;
+			}
+			contactIndex++;
+		}
+	}
+}
 
 struct AllocContactConstraintsLoop : public btIParallelForBody
 {
-    btSequentialImpulseConstraintSolverMt* m_solver;
-    const btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray;
-
-    AllocContactConstraintsLoop( btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray )
-    {
-        m_solver = solver;
-        m_cachedInfoArray = cachedInfoArray;
-    }
-    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
-    {
-        m_solver->internalAllocContactConstraints( m_cachedInfoArray + iBegin, iEnd - iBegin );
-    }
-};
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray;
 
+	AllocContactConstraintsLoop(btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray)
+	{
+		m_solver = solver;
+		m_cachedInfoArray = cachedInfoArray;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalAllocContactConstraints(m_cachedInfoArray + iBegin, iEnd - iBegin);
+	}
+};
 
 void btSequentialImpulseConstraintSolverMt::allocAllContactConstraints(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
 {
-    BT_PROFILE( "allocAllContactConstraints" );
-    btAlignedObjectArray<btContactManifoldCachedInfo> cachedInfoArray; // = m_manifoldCachedInfoArray;
-    cachedInfoArray.resizeNoInitialize( numManifolds );
-    if (/* DISABLES CODE */ (false))
-    {
-        // sequential
-        internalCollectContactManifoldCachedInfo(&cachedInfoArray[ 0 ], manifoldPtr, numManifolds, infoGlobal);
-    }
-    else
-    {
-        // may alter ordering of bodies which affects determinism
-        CollectContactManifoldCachedInfoLoop loop( this, &cachedInfoArray[ 0 ], manifoldPtr, infoGlobal );
-        int grainSize = 200;
-        btParallelFor( 0, numManifolds, grainSize, loop );
-    }
-
-    {
-        // serial part
-        int numContacts = 0;
-        int numRollingFrictionConstraints = 0;
-        for ( int iManifold = 0; iManifold < numManifolds; ++iManifold )
-        {
-            btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[ iManifold ];
-            cachedInfo.contactIndex = numContacts;
-            cachedInfo.rollingFrictionIndex = numRollingFrictionConstraints;
-            numContacts += cachedInfo.numTouchingContacts;
-            for (int i = 0; i < cachedInfo.numTouchingContacts; ++i)
-            {
-                if (cachedInfo.contactHasRollingFriction[i])
-                {
-                    numRollingFrictionConstraints += 3;
-                }
-            }
-        }
-        {
-            BT_PROFILE( "allocPools" );
-            if ( m_tmpSolverContactConstraintPool.capacity() < numContacts )
-            {
-                // if we need to reallocate, reserve some extra so we don't have to reallocate again next frame
-                int extraReserve = numContacts / 16;
-                m_tmpSolverContactConstraintPool.reserve( numContacts + extraReserve );
-                m_rollingFrictionIndexTable.reserve( numContacts + extraReserve );
-                m_tmpSolverContactFrictionConstraintPool.reserve( ( numContacts + extraReserve )*m_numFrictionDirections );
-                m_tmpSolverContactRollingFrictionConstraintPool.reserve( numRollingFrictionConstraints + extraReserve );
-            }
-            m_tmpSolverContactConstraintPool.resizeNoInitialize( numContacts );
-            m_rollingFrictionIndexTable.resizeNoInitialize( numContacts );
-            m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize( numContacts*m_numFrictionDirections );
-            m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize( numRollingFrictionConstraints );
-        }
-    }
-    {
-        AllocContactConstraintsLoop loop(this, &cachedInfoArray[0]);
-        int grainSize = 200;
-        btParallelFor( 0, numManifolds, grainSize, loop );
-    }
-}
+	BT_PROFILE("allocAllContactConstraints");
+	btAlignedObjectArray<btContactManifoldCachedInfo> cachedInfoArray;  // = m_manifoldCachedInfoArray;
+	cachedInfoArray.resizeNoInitialize(numManifolds);
+	if (/* DISABLES CODE */ (false))
+	{
+		// sequential
+		internalCollectContactManifoldCachedInfo(&cachedInfoArray[0], manifoldPtr, numManifolds, infoGlobal);
+	}
+	else
+	{
+		// may alter ordering of bodies which affects determinism
+		CollectContactManifoldCachedInfoLoop loop(this, &cachedInfoArray[0], manifoldPtr, infoGlobal);
+		int grainSize = 200;
+		btParallelFor(0, numManifolds, grainSize, loop);
+	}
 
+	{
+		// serial part
+		int numContacts = 0;
+		int numRollingFrictionConstraints = 0;
+		for (int iManifold = 0; iManifold < numManifolds; ++iManifold)
+		{
+			btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[iManifold];
+			cachedInfo.contactIndex = numContacts;
+			cachedInfo.rollingFrictionIndex = numRollingFrictionConstraints;
+			numContacts += cachedInfo.numTouchingContacts;
+			for (int i = 0; i < cachedInfo.numTouchingContacts; ++i)
+			{
+				if (cachedInfo.contactHasRollingFriction[i])
+				{
+					numRollingFrictionConstraints += 3;
+				}
+			}
+		}
+		{
+			BT_PROFILE("allocPools");
+			if (m_tmpSolverContactConstraintPool.capacity() < numContacts)
+			{
+				// if we need to reallocate, reserve some extra so we don't have to reallocate again next frame
+				int extraReserve = numContacts / 16;
+				m_tmpSolverContactConstraintPool.reserve(numContacts + extraReserve);
+				m_rollingFrictionIndexTable.reserve(numContacts + extraReserve);
+				m_tmpSolverContactFrictionConstraintPool.reserve((numContacts + extraReserve) * m_numFrictionDirections);
+				m_tmpSolverContactRollingFrictionConstraintPool.reserve(numRollingFrictionConstraints + extraReserve);
+			}
+			m_tmpSolverContactConstraintPool.resizeNoInitialize(numContacts);
+			m_rollingFrictionIndexTable.resizeNoInitialize(numContacts);
+			m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(numContacts * m_numFrictionDirections);
+			m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(numRollingFrictionConstraints);
+		}
+	}
+	{
+		AllocContactConstraintsLoop loop(this, &cachedInfoArray[0]);
+		int grainSize = 200;
+		btParallelFor(0, numManifolds, grainSize, loop);
+	}
+}
 
 void btSequentialImpulseConstraintSolverMt::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
 {
-    if (!m_useBatching)
-    {
-        btSequentialImpulseConstraintSolver::convertContacts(manifoldPtr, numManifolds, infoGlobal);
-        return;
-    }
-    BT_PROFILE( "convertContacts" );
-    if (numManifolds > 0)
-    {
-        if ( m_fixedBodyId < 0 )
-        {
-            m_fixedBodyId = m_tmpSolverBodyPool.size();
-            btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
-            initSolverBody( &fixedBody, 0, infoGlobal.m_timeStep );
-        }
-        allocAllContactConstraints( manifoldPtr, numManifolds, infoGlobal );
-        if ( m_useBatching )
-        {
-            setupBatchedContactConstraints();
-        }
-        setupAllContactConstraints( infoGlobal );
-    }
+	if (!m_useBatching)
+	{
+		btSequentialImpulseConstraintSolver::convertContacts(manifoldPtr, numManifolds, infoGlobal);
+		return;
+	}
+	BT_PROFILE("convertContacts");
+	if (numManifolds > 0)
+	{
+		if (m_fixedBodyId < 0)
+		{
+			m_fixedBodyId = m_tmpSolverBodyPool.size();
+			btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+			initSolverBody(&fixedBody, 0, infoGlobal.m_timeStep);
+		}
+		allocAllContactConstraints(manifoldPtr, numManifolds, infoGlobal);
+		if (m_useBatching)
+		{
+			setupBatchedContactConstraints();
+		}
+		setupAllContactConstraints(infoGlobal);
+	}
 }
 
-
-void btSequentialImpulseConstraintSolverMt::internalInitMultipleJoints( btTypedConstraint** constraints, int iBegin, int iEnd )
+void btSequentialImpulseConstraintSolverMt::internalInitMultipleJoints(btTypedConstraint** constraints, int iBegin, int iEnd)
 {
-    BT_PROFILE("internalInitMultipleJoints");
-    for ( int i = iBegin; i < iEnd; i++ )
+	BT_PROFILE("internalInitMultipleJoints");
+	for (int i = iBegin; i < iEnd; i++)
 	{
 		btTypedConstraint* constraint = constraints[i];
 		btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
 		if (constraint->isEnabled())
-        {
-            constraint->buildJacobian();
-            constraint->internalSetAppliedImpulse( 0.0f );
-            btJointFeedback* fb = constraint->getJointFeedback();
-            if ( fb )
-            {
-                fb->m_appliedForceBodyA.setZero();
-                fb->m_appliedTorqueBodyA.setZero();
-                fb->m_appliedForceBodyB.setZero();
-                fb->m_appliedTorqueBodyB.setZero();
-            }
-            constraint->getInfo1( &info1 );
-        }
-        else
+		{
+			constraint->buildJacobian();
+			constraint->internalSetAppliedImpulse(0.0f);
+			btJointFeedback* fb = constraint->getJointFeedback();
+			if (fb)
+			{
+				fb->m_appliedForceBodyA.setZero();
+				fb->m_appliedTorqueBodyA.setZero();
+				fb->m_appliedForceBodyB.setZero();
+				fb->m_appliedTorqueBodyB.setZero();
+			}
+			constraint->getInfo1(&info1);
+		}
+		else
 		{
 			info1.m_numConstraintRows = 0;
 			info1.nub = 0;
@@ -646,158 +624,151 @@ void btSequentialImpulseConstraintSolverMt::internalInitMultipleJoints( btTypedC
 	}
 }
 
-
 struct InitJointsLoop : public btIParallelForBody
 {
-    btSequentialImpulseConstraintSolverMt* m_solver;
-    btTypedConstraint** m_constraints;
-
-    InitJointsLoop( btSequentialImpulseConstraintSolverMt* solver, btTypedConstraint** constraints )
-    {
-        m_solver = solver;
-        m_constraints = constraints;
-    }
-    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
-    {
-        m_solver->internalInitMultipleJoints( m_constraints, iBegin, iEnd );
-    }
-};
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	btTypedConstraint** m_constraints;
 
+	InitJointsLoop(btSequentialImpulseConstraintSolverMt* solver, btTypedConstraint** constraints)
+	{
+		m_solver = solver;
+		m_constraints = constraints;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalInitMultipleJoints(m_constraints, iBegin, iEnd);
+	}
+};
 
-void btSequentialImpulseConstraintSolverMt::internalConvertMultipleJoints( const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal )
+void btSequentialImpulseConstraintSolverMt::internalConvertMultipleJoints(const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
 {
-    BT_PROFILE("internalConvertMultipleJoints");
-    for ( int i = iBegin; i < iEnd; ++i )
-    {
-        const JointParams& jointParams = jointParamsArray[ i ];
-        int currentRow = jointParams.m_solverConstraint;
-        if ( currentRow != -1 )
-        {
-            const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[ i ];
-            btAssert( currentRow < m_tmpSolverNonContactConstraintPool.size() );
-            btAssert( info1.m_numConstraintRows > 0 );
-
-            btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[ currentRow ];
-            btTypedConstraint* constraint = constraints[ i ];
-
-            convertJoint( currentConstraintRow, constraint, info1, jointParams.m_solverBodyA, jointParams.m_solverBodyB, infoGlobal );
-        }
-    }
-}
+	BT_PROFILE("internalConvertMultipleJoints");
+	for (int i = iBegin; i < iEnd; ++i)
+	{
+		const JointParams& jointParams = jointParamsArray[i];
+		int currentRow = jointParams.m_solverConstraint;
+		if (currentRow != -1)
+		{
+			const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+			btAssert(currentRow < m_tmpSolverNonContactConstraintPool.size());
+			btAssert(info1.m_numConstraintRows > 0);
+
+			btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
+			btTypedConstraint* constraint = constraints[i];
 
+			convertJoint(currentConstraintRow, constraint, info1, jointParams.m_solverBodyA, jointParams.m_solverBodyB, infoGlobal);
+		}
+	}
+}
 
 struct ConvertJointsLoop : public btIParallelForBody
 {
-    btSequentialImpulseConstraintSolverMt* m_solver;
-    const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& m_jointParamsArray;
-    btTypedConstraint** m_srcConstraints;
-    const btContactSolverInfo& m_infoGlobal;
-
-    ConvertJointsLoop( btSequentialImpulseConstraintSolverMt* solver,
-        const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& jointParamsArray,
-        btTypedConstraint** srcConstraints,
-        const btContactSolverInfo& infoGlobal
-    ) :
-        m_jointParamsArray(jointParamsArray),
-        m_infoGlobal(infoGlobal)
-    {
-        m_solver = solver;
-        m_srcConstraints = srcConstraints;
-    }
-    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
-    {
-        m_solver->internalConvertMultipleJoints( m_jointParamsArray, m_srcConstraints, iBegin, iEnd, m_infoGlobal );
-    }
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& m_jointParamsArray;
+	btTypedConstraint** m_srcConstraints;
+	const btContactSolverInfo& m_infoGlobal;
+
+	ConvertJointsLoop(btSequentialImpulseConstraintSolverMt* solver,
+					  const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& jointParamsArray,
+					  btTypedConstraint** srcConstraints,
+					  const btContactSolverInfo& infoGlobal) : m_jointParamsArray(jointParamsArray),
+															   m_infoGlobal(infoGlobal)
+	{
+		m_solver = solver;
+		m_srcConstraints = srcConstraints;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalConvertMultipleJoints(m_jointParamsArray, m_srcConstraints, iBegin, iEnd, m_infoGlobal);
+	}
 };
 
-
 void btSequentialImpulseConstraintSolverMt::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal)
 {
-    if ( !m_useBatching )
-    {
-        btSequentialImpulseConstraintSolver::convertJoints(constraints, numConstraints, infoGlobal);
-        return;
-    }
-    BT_PROFILE("convertJoints");
-    bool parallelJointSetup = true;
+	if (!m_useBatching)
+	{
+		btSequentialImpulseConstraintSolver::convertJoints(constraints, numConstraints, infoGlobal);
+		return;
+	}
+	BT_PROFILE("convertJoints");
+	bool parallelJointSetup = true;
 	m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
-    if (parallelJointSetup)
-    {
-        InitJointsLoop loop(this, constraints);
-        int grainSize = 40;
-        btParallelFor(0, numConstraints, grainSize, loop);
-    }
-    else
-    {
-        internalInitMultipleJoints( constraints, 0, numConstraints );
-    }
+	if (parallelJointSetup)
+	{
+		InitJointsLoop loop(this, constraints);
+		int grainSize = 40;
+		btParallelFor(0, numConstraints, grainSize, loop);
+	}
+	else
+	{
+		internalInitMultipleJoints(constraints, 0, numConstraints);
+	}
 
 	int totalNumRows = 0;
-    btAlignedObjectArray<JointParams> jointParamsArray;
-    jointParamsArray.resizeNoInitialize(numConstraints);
+	btAlignedObjectArray<JointParams> jointParamsArray;
+	jointParamsArray.resizeNoInitialize(numConstraints);
 
 	//calculate the total number of contraint rows
-	for (int i=0;i<numConstraints;i++)
+	for (int i = 0; i < numConstraints; i++)
 	{
-        btTypedConstraint* constraint = constraints[ i ];
+		btTypedConstraint* constraint = constraints[i];
 
-        JointParams& params = jointParamsArray[ i ];
+		JointParams& params = jointParamsArray[i];
 		const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
 
 		if (info1.m_numConstraintRows)
 		{
-            params.m_solverConstraint = totalNumRows;
-            params.m_solverBodyA = getOrInitSolverBody( constraint->getRigidBodyA(), infoGlobal.m_timeStep );
-            params.m_solverBodyB = getOrInitSolverBody( constraint->getRigidBodyB(), infoGlobal.m_timeStep );
+			params.m_solverConstraint = totalNumRows;
+			params.m_solverBodyA = getOrInitSolverBody(constraint->getRigidBodyA(), infoGlobal.m_timeStep);
+			params.m_solverBodyB = getOrInitSolverBody(constraint->getRigidBodyB(), infoGlobal.m_timeStep);
 		}
-        else
+		else
 		{
-            params.m_solverConstraint = -1;
+			params.m_solverConstraint = -1;
 		}
 		totalNumRows += info1.m_numConstraintRows;
 	}
 	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
 
 	///setup the btSolverConstraints
-    if ( parallelJointSetup )
-    {
-        ConvertJointsLoop loop(this, jointParamsArray, constraints, infoGlobal);
-        int grainSize = 20;
-        btParallelFor(0, numConstraints, grainSize, loop);
-    }
-    else
-    {
-        internalConvertMultipleJoints( jointParamsArray, constraints, 0, numConstraints, infoGlobal );
-    }
-    setupBatchedJointConstraints();
+	if (parallelJointSetup)
+	{
+		ConvertJointsLoop loop(this, jointParamsArray, constraints, infoGlobal);
+		int grainSize = 20;
+		btParallelFor(0, numConstraints, grainSize, loop);
+	}
+	else
+	{
+		internalConvertMultipleJoints(jointParamsArray, constraints, 0, numConstraints, infoGlobal);
+	}
+	setupBatchedJointConstraints();
 }
 
-
 void btSequentialImpulseConstraintSolverMt::internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
 {
-    BT_PROFILE("internalConvertBodies");
-    for (int i=iBegin; i < iEnd; i++)
+	BT_PROFILE("internalConvertBodies");
+	for (int i = iBegin; i < iEnd; i++)
 	{
-        btCollisionObject* obj = bodies[i];
+		btCollisionObject* obj = bodies[i];
 		obj->setCompanionId(i);
 		btSolverBody& solverBody = m_tmpSolverBodyPool[i];
-        initSolverBody(&solverBody, obj, infoGlobal.m_timeStep);
+		initSolverBody(&solverBody, obj, infoGlobal.m_timeStep);
 
 		btRigidBody* body = btRigidBody::upcast(obj);
 		if (body && body->getInvMass())
 		{
-			btVector3 gyroForce (0,0,0);
-			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
+			btVector3 gyroForce(0, 0, 0);
+			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
 			{
 				gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
-				solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
+				solverBody.m_externalTorqueImpulse -= gyroForce * body->getInvInertiaTensorWorld() * infoGlobal.m_timeStep;
 			}
-			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
+			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
 			{
 				gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
 				solverBody.m_externalTorqueImpulse += gyroForce;
 			}
-			if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
+			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
 			{
 				gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
 				solverBody.m_externalTorqueImpulse += gyroForce;
@@ -806,809 +777,772 @@ void btSequentialImpulseConstraintSolverMt::internalConvertBodies(btCollisionObj
 	}
 }
 
-
 struct ConvertBodiesLoop : public btIParallelForBody
 {
-    btSequentialImpulseConstraintSolverMt* m_solver;
-    btCollisionObject** m_bodies;
-    int m_numBodies;
-    const btContactSolverInfo& m_infoGlobal;
-
-    ConvertBodiesLoop( btSequentialImpulseConstraintSolverMt* solver,
-        btCollisionObject** bodies,
-        int numBodies,
-        const btContactSolverInfo& infoGlobal
-    ) :
-        m_infoGlobal(infoGlobal)
-    {
-        m_solver = solver;
-        m_bodies = bodies;
-        m_numBodies = numBodies;
-    }
-    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
-    {
-        m_solver->internalConvertBodies( m_bodies, iBegin, iEnd, m_infoGlobal );
-    }
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	btCollisionObject** m_bodies;
+	int m_numBodies;
+	const btContactSolverInfo& m_infoGlobal;
+
+	ConvertBodiesLoop(btSequentialImpulseConstraintSolverMt* solver,
+					  btCollisionObject** bodies,
+					  int numBodies,
+					  const btContactSolverInfo& infoGlobal) : m_infoGlobal(infoGlobal)
+	{
+		m_solver = solver;
+		m_bodies = bodies;
+		m_numBodies = numBodies;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalConvertBodies(m_bodies, iBegin, iEnd, m_infoGlobal);
+	}
 };
 
-
 void btSequentialImpulseConstraintSolverMt::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
 {
-    BT_PROFILE("convertBodies");
-    m_kinematicBodyUniqueIdToSolverBodyTable.resize( 0 );
-
-	m_tmpSolverBodyPool.resizeNoInitialize(numBodies+1);
-
-    m_fixedBodyId = numBodies;
-    {
-        btSolverBody& fixedBody = m_tmpSolverBodyPool[ m_fixedBodyId ];
-        initSolverBody( &fixedBody, NULL, infoGlobal.m_timeStep );
-    }
-
-    bool parallelBodySetup = true;
-    if (parallelBodySetup)
-    {
-        ConvertBodiesLoop loop(this, bodies, numBodies, infoGlobal);
-        int grainSize = 40;
-        btParallelFor(0, numBodies, grainSize, loop);
-    }
-    else
-    {
-        internalConvertBodies( bodies, 0, numBodies, infoGlobal );
-    }
-}
+	BT_PROFILE("convertBodies");
+	m_kinematicBodyUniqueIdToSolverBodyTable.resize(0);
 
+	m_tmpSolverBodyPool.resizeNoInitialize(numBodies + 1);
+
+	m_fixedBodyId = numBodies;
+	{
+		btSolverBody& fixedBody = m_tmpSolverBodyPool[m_fixedBodyId];
+		initSolverBody(&fixedBody, NULL, infoGlobal.m_timeStep);
+	}
+
+	bool parallelBodySetup = true;
+	if (parallelBodySetup)
+	{
+		ConvertBodiesLoop loop(this, bodies, numBodies, infoGlobal);
+		int grainSize = 40;
+		btParallelFor(0, numBodies, grainSize, loop);
+	}
+	else
+	{
+		internalConvertBodies(bodies, 0, numBodies, infoGlobal);
+	}
+}
 
 btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySetup(
-     btCollisionObject** bodies,
-     int numBodies,
-     btPersistentManifold** manifoldPtr,
-     int numManifolds,
-     btTypedConstraint** constraints,
-     int numConstraints,
-     const btContactSolverInfo& infoGlobal,
-     btIDebugDraw* debugDrawer
-     )
+	btCollisionObject** bodies,
+	int numBodies,
+	btPersistentManifold** manifoldPtr,
+	int numManifolds,
+	btTypedConstraint** constraints,
+	int numConstraints,
+	const btContactSolverInfo& infoGlobal,
+	btIDebugDraw* debugDrawer)
 {
-    m_numFrictionDirections = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
-    m_useBatching = false;
-    if ( numManifolds >= s_minimumContactManifoldsForBatching &&
-        (s_allowNestedParallelForLoops || !btThreadsAreRunning())
-        )
-    {
-        m_useBatching = true;
-        m_batchedContactConstraints.m_debugDrawer = debugDrawer;
-        m_batchedJointConstraints.m_debugDrawer = debugDrawer;
-    }
-    btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,
-                                                                       numBodies,
-                                                                       manifoldPtr,
-                                                                       numManifolds,
-                                                                       constraints,
-                                                                       numConstraints,
-                                                                       infoGlobal,
-                                                                       debugDrawer
-                                                                       );
-    return 0.0f;
+	m_numFrictionDirections = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
+	m_useBatching = false;
+	if (numManifolds >= s_minimumContactManifoldsForBatching &&
+		(s_allowNestedParallelForLoops || !btThreadsAreRunning()))
+	{
+		m_useBatching = true;
+		m_batchedContactConstraints.m_debugDrawer = debugDrawer;
+		m_batchedJointConstraints.m_debugDrawer = debugDrawer;
+	}
+	btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies,
+																	  numBodies,
+																	  manifoldPtr,
+																	  numManifolds,
+																	  constraints,
+																	  numConstraints,
+																	  infoGlobal,
+																	  debugDrawer);
+	return 0.0f;
 }
 
-
-btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactSplitPenetrationImpulseConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd )
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactSplitPenetrationImpulseConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
 {
-    btScalar leastSquaresResidual = 0.f;
-    for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons )
-    {
-        int iCons = consIndices[ iiCons ];
-        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[ iCons ];
-        btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ];
-        btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ];
-        btScalar residual = resolveSplitPenetrationImpulse( bodyA, bodyB, solveManifold );
-        leastSquaresResidual += residual*residual;
-    }
-    return leastSquaresResidual;
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
+	{
+		int iCons = consIndices[iiCons];
+		const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iCons];
+		btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
+		btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
+		btScalar residual = resolveSplitPenetrationImpulse(bodyA, bodyB, solveManifold);
+		leastSquaresResidual += residual * residual;
+	}
+	return leastSquaresResidual;
 }
 
-
 struct ContactSplitPenetrationImpulseSolverLoop : public btIParallelSumBody
 {
-    btSequentialImpulseConstraintSolverMt* m_solver;
-    const btBatchedConstraints* m_bc;
-
-    ContactSplitPenetrationImpulseSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc )
-    {
-        m_solver = solver;
-        m_bc = bc;
-    }
-    btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE
-    {
-        BT_PROFILE( "ContactSplitPenetrationImpulseSolverLoop" );
-        btScalar sum = 0;
-        for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch )
-        {
-            const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ];
-            sum += m_solver->resolveMultipleContactSplitPenetrationImpulseConstraints( m_bc->m_constraintIndices, batch.begin, batch.end );
-        }
-        return sum;
-    }
-};
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
 
+	ContactSplitPenetrationImpulseSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
+	{
+		m_solver = solver;
+		m_bc = bc;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("ContactSplitPenetrationImpulseSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleContactSplitPenetrationImpulseConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
+		}
+		return sum;
+	}
+};
 
-void btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+void btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
 	if (infoGlobal.m_splitImpulse)
 	{
-        for ( int iteration = 0; iteration < infoGlobal.m_numIterations; iteration++ )
-        {
-            btScalar leastSquaresResidual = 0.f;
-            if (m_useBatching)
-            {
-                const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
-                ContactSplitPenetrationImpulseSolverLoop loop( this, &batchedCons );
-                btScalar leastSquaresResidual = 0.f;
-                for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase )
-                {
-                    int iPhase = batchedCons.m_phaseOrder[ iiPhase ];
-                    const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ];
-                    int grainSize = batchedCons.m_phaseGrainSize[iPhase];
-                    leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop );
-                }
-            }
-            else
-            {
-                // non-batched
-                leastSquaresResidual = resolveMultipleContactSplitPenetrationImpulseConstraints(m_orderTmpConstraintPool, 0, m_tmpSolverContactConstraintPool.size());
-            }
-            if ( leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= ( infoGlobal.m_numIterations - 1 ) )
-            {
+		for (int iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
+		{
+			btScalar leastSquaresResidual = 0.f;
+			if (m_useBatching)
+			{
+				const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+				ContactSplitPenetrationImpulseSolverLoop loop(this, &batchedCons);
+				btScalar leastSquaresResidual = 0.f;
+				for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+				{
+					int iPhase = batchedCons.m_phaseOrder[iiPhase];
+					const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+					int grainSize = batchedCons.m_phaseGrainSize[iPhase];
+					leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+				}
+			}
+			else
+			{
+				// non-batched
+				leastSquaresResidual = resolveMultipleContactSplitPenetrationImpulseConstraints(m_orderTmpConstraintPool, 0, m_tmpSolverContactConstraintPool.size());
+			}
+			if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
+			{
 #ifdef VERBOSE_RESIDUAL_PRINTF
-                printf( "residual = %f at iteration #%d\n", leastSquaresResidual, iteration );
+				printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
 #endif
-                break;
-            }
-        }
+				break;
+			}
+		}
 	}
 }
 
-
-btScalar btSequentialImpulseConstraintSolverMt::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+btScalar btSequentialImpulseConstraintSolverMt::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
-    if ( !m_useBatching )
-    {
-        return btSequentialImpulseConstraintSolver::solveSingleIteration( iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer );
-    }
-    BT_PROFILE( "solveSingleIterationMt" );
-    btScalar leastSquaresResidual = 0.f;
+	if (!m_useBatching)
+	{
+		return btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+	}
+	BT_PROFILE("solveSingleIterationMt");
+	btScalar leastSquaresResidual = 0.f;
 
 	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
 	{
-		if (1)			// uncomment this for a bit less random ((iteration & 7) == 0)
+		if (1)  // uncomment this for a bit less random ((iteration & 7) == 0)
 		{
-            randomizeConstraintOrdering(iteration, infoGlobal.m_numIterations);
+			randomizeConstraintOrdering(iteration, infoGlobal.m_numIterations);
 		}
 	}
 
 	{
 		///solve all joint constraints
-        leastSquaresResidual += resolveAllJointConstraints(iteration);
+		leastSquaresResidual += resolveAllJointConstraints(iteration);
 
-		if (iteration< infoGlobal.m_numIterations)
+		if (iteration < infoGlobal.m_numIterations)
 		{
-            // this loop is only used for cone-twist constraints,
-            // it would be nice to skip this loop if none of the constraints need it
-            if ( m_useObsoleteJointConstraints )
-            {
-                for ( int j = 0; j<numConstraints; j++ )
-                {
-                    if ( constraints[ j ]->isEnabled() )
-                    {
-                        int bodyAid = getOrInitSolverBody( constraints[ j ]->getRigidBodyA(), infoGlobal.m_timeStep );
-                        int bodyBid = getOrInitSolverBody( constraints[ j ]->getRigidBodyB(), infoGlobal.m_timeStep );
-                        btSolverBody& bodyA = m_tmpSolverBodyPool[ bodyAid ];
-                        btSolverBody& bodyB = m_tmpSolverBodyPool[ bodyBid ];
-                        constraints[ j ]->solveConstraintObsolete( bodyA, bodyB, infoGlobal.m_timeStep );
-                    }
-                }
-            }
+			// this loop is only used for cone-twist constraints,
+			// it would be nice to skip this loop if none of the constraints need it
+			if (m_useObsoleteJointConstraints)
+			{
+				for (int j = 0; j < numConstraints; j++)
+				{
+					if (constraints[j]->isEnabled())
+					{
+						int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(), infoGlobal.m_timeStep);
+						int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(), infoGlobal.m_timeStep);
+						btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+						btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+						constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
+					}
+				}
+			}
 
 			if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
 			{
-                // solve all contact, contact-friction, and rolling friction constraints interleaved
-                leastSquaresResidual += resolveAllContactConstraintsInterleaved();
+				// solve all contact, contact-friction, and rolling friction constraints interleaved
+				leastSquaresResidual += resolveAllContactConstraintsInterleaved();
 			}
-			else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
+			else  //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
 			{
-                // don't interleave them
+				// don't interleave them
 				// solve all contact constraints
-                leastSquaresResidual += resolveAllContactConstraints();
+				leastSquaresResidual += resolveAllContactConstraints();
 
 				// solve all contact friction constraints
-                leastSquaresResidual += resolveAllContactFrictionConstraints();
+				leastSquaresResidual += resolveAllContactFrictionConstraints();
 
-                // solve all rolling friction constraints
-                leastSquaresResidual += resolveAllRollingFrictionConstraints();
+				// solve all rolling friction constraints
+				leastSquaresResidual += resolveAllRollingFrictionConstraints();
 			}
 		}
 	}
-    return leastSquaresResidual;
+	return leastSquaresResidual;
 }
 
-
-btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleJointConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration )
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleJointConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration)
 {
-    btScalar leastSquaresResidual = 0.f;
-    for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons )
-    {
-        int iCons = consIndices[ iiCons ];
-        const btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[ iCons ];
-        if ( iteration < constraint.m_overrideNumSolverIterations )
-        {
-            btSolverBody& bodyA = m_tmpSolverBodyPool[ constraint.m_solverBodyIdA ];
-            btSolverBody& bodyB = m_tmpSolverBodyPool[ constraint.m_solverBodyIdB ];
-            btScalar residual = resolveSingleConstraintRowGeneric( bodyA, bodyB, constraint );
-            leastSquaresResidual += residual*residual;
-        }
-    }
-    return leastSquaresResidual;
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
+	{
+		int iCons = consIndices[iiCons];
+		const btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[iCons];
+		if (iteration < constraint.m_overrideNumSolverIterations)
+		{
+			btSolverBody& bodyA = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+			btSolverBody& bodyB = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+			btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, constraint);
+			leastSquaresResidual += residual * residual;
+		}
+	}
+	return leastSquaresResidual;
 }
 
-
-btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd )
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
 {
-    btScalar leastSquaresResidual = 0.f;
-    for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons )
-    {
-        int iCons = consIndices[ iiCons ];
-        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[ iCons ];
-        btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ];
-        btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ];
-        btScalar residual = resolveSingleConstraintRowLowerLimit( bodyA, bodyB, solveManifold );
-        leastSquaresResidual += residual*residual;
-    }
-    return leastSquaresResidual;
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
+	{
+		int iCons = consIndices[iiCons];
+		const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iCons];
+		btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
+		btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
+		btScalar residual = resolveSingleConstraintRowLowerLimit(bodyA, bodyB, solveManifold);
+		leastSquaresResidual += residual * residual;
+	}
+	return leastSquaresResidual;
 }
 
-
-btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd )
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
 {
-    btScalar leastSquaresResidual = 0.f;
-    for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons )
-    {
-        int iContact = consIndices[ iiCons ];
-        btScalar totalImpulse = m_tmpSolverContactConstraintPool[ iContact ].m_appliedImpulse;
-
-        // apply sliding friction
-        if ( totalImpulse > 0.0f )
-        {
-            int iBegin = iContact * m_numFrictionDirections;
-            int iEnd = iBegin + m_numFrictionDirections;
-            for ( int iFriction = iBegin; iFriction < iEnd; ++iFriction )
-            {
-                btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[ iFriction++ ];
-                btAssert( solveManifold.m_frictionIndex == iContact );
-
-                solveManifold.m_lowerLimit = -( solveManifold.m_friction*totalImpulse );
-                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-
-                btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ];
-                btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ];
-                btScalar residual = resolveSingleConstraintRowGeneric( bodyA, bodyB, solveManifold );
-                leastSquaresResidual += residual*residual;
-            }
-        }
-    }
-    return leastSquaresResidual;
-}
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
+	{
+		int iContact = consIndices[iiCons];
+		btScalar totalImpulse = m_tmpSolverContactConstraintPool[iContact].m_appliedImpulse;
 
+		// apply sliding friction
+		if (totalImpulse > 0.0f)
+		{
+			int iBegin = iContact * m_numFrictionDirections;
+			int iEnd = iBegin + m_numFrictionDirections;
+			for (int iFriction = iBegin; iFriction < iEnd; ++iFriction)
+			{
+				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[iFriction++];
+				btAssert(solveManifold.m_frictionIndex == iContact);
 
-btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactRollingFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd )
-{
-    btScalar leastSquaresResidual = 0.f;
-    for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons )
-    {
-        int iContact = consIndices[ iiCons ];
-        int iFirstRollingFriction = m_rollingFrictionIndexTable[ iContact ];
-        if ( iFirstRollingFriction >= 0 )
-        {
-            btScalar totalImpulse = m_tmpSolverContactConstraintPool[ iContact ].m_appliedImpulse;
-            // apply rolling friction
-            if ( totalImpulse > 0.0f )
-            {
-                int iBegin = iFirstRollingFriction;
-                int iEnd = iBegin + 3;
-                for ( int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric )
-                {
-                    btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ iRollingFric ];
-                    if ( rollingFrictionConstraint.m_frictionIndex != iContact )
-                    {
-                        break;
-                    }
-                    btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
-                    if ( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction )
-                    {
-                        rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
-                    }
-
-                    rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
-                    rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
-
-                    btScalar residual = resolveSingleConstraintRowGeneric( m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdA ], m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdB ], rollingFrictionConstraint );
-                    leastSquaresResidual += residual*residual;
-                }
-            }
-        }
-    }
-    return leastSquaresResidual;
-}
+				solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+				solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
 
+				btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
+				btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
+				btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, solveManifold);
+				leastSquaresResidual += residual * residual;
+			}
+		}
+	}
+	return leastSquaresResidual;
+}
 
-btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray<int>& contactIndices,
-                                                                                          int batchBegin,
-                                                                                          int batchEnd
-                                                                                          )
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactRollingFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
 {
-    btScalar leastSquaresResidual = 0.f;
-    int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-
-    for ( int iiCons = batchBegin; iiCons < batchEnd; iiCons++ )
-    {
-        btScalar totalImpulse = 0;
-        int iContact = contactIndices[ iiCons ];
-        // apply penetration constraint
-        {
-            const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[ iContact ];
-            btScalar residual = resolveSingleConstraintRowLowerLimit( m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ], m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ], solveManifold );
-            leastSquaresResidual += residual*residual;
-            totalImpulse = solveManifold.m_appliedImpulse;
-        }
-
-        // apply sliding friction
-        if ( totalImpulse > 0.0f )
-        {
-            int iBegin = iContact * m_numFrictionDirections;
-            int iEnd = iBegin + m_numFrictionDirections;
-            for ( int iFriction = iBegin; iFriction < iEnd; ++iFriction )
-            {
-                btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[ iFriction ];
-                btAssert( solveManifold.m_frictionIndex == iContact );
-
-                solveManifold.m_lowerLimit = -( solveManifold.m_friction*totalImpulse );
-                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-
-                btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ];
-                btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ];
-                btScalar residual = resolveSingleConstraintRowGeneric( bodyA, bodyB, solveManifold );
-                leastSquaresResidual += residual*residual;
-            }
-        }
-
-        // apply rolling friction
-        int iFirstRollingFriction = m_rollingFrictionIndexTable[ iContact ];
-        if ( totalImpulse > 0.0f && iFirstRollingFriction >= 0)
-        {
-            int iBegin = iFirstRollingFriction;
-            int iEnd = iBegin + 3;
-            for ( int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric )
-            {
-                btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ iRollingFric ];
-                if ( rollingFrictionConstraint.m_frictionIndex != iContact )
-                {
-                    break;
-                }
-                btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
-                if ( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction )
-                {
-                    rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
-                }
-
-                rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
-                rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
-
-                btScalar residual = resolveSingleConstraintRowGeneric( m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdA ], m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdB ], rollingFrictionConstraint );
-                leastSquaresResidual += residual*residual;
-            }
-        }
-    }
-    return leastSquaresResidual;
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
+	{
+		int iContact = consIndices[iiCons];
+		int iFirstRollingFriction = m_rollingFrictionIndexTable[iContact];
+		if (iFirstRollingFriction >= 0)
+		{
+			btScalar totalImpulse = m_tmpSolverContactConstraintPool[iContact].m_appliedImpulse;
+			// apply rolling friction
+			if (totalImpulse > 0.0f)
+			{
+				int iBegin = iFirstRollingFriction;
+				int iEnd = iBegin + 3;
+				for (int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric)
+				{
+					btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric];
+					if (rollingFrictionConstraint.m_frictionIndex != iContact)
+					{
+						break;
+					}
+					btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
+					if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
+					{
+						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+					}
+
+					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+					btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
+					leastSquaresResidual += residual * residual;
+				}
+			}
+		}
+	}
+	return leastSquaresResidual;
 }
 
-
-void btSequentialImpulseConstraintSolverMt::randomizeBatchedConstraintOrdering( btBatchedConstraints* batchedConstraints )
+btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraintsInterleaved(const btAlignedObjectArray<int>& contactIndices,
+																							 int batchBegin,
+																							 int batchEnd)
 {
-    btBatchedConstraints& bc = *batchedConstraints;
-    // randomize ordering of phases
-    for ( int ii = 1; ii < bc.m_phaseOrder.size(); ++ii )
-    {
-        int iSwap = btRandInt2( ii + 1 );
-        bc.m_phaseOrder.swap( ii, iSwap );
-    }
-
-    // for each batch,
-    for ( int iBatch = 0; iBatch < bc.m_batches.size(); ++iBatch )
-    {
-        // randomize ordering of constraints within the batch
-        const btBatchedConstraints::Range& batch = bc.m_batches[ iBatch ];
-        for ( int iiCons = batch.begin; iiCons < batch.end; ++iiCons )
-        {
-            int iSwap = batch.begin + btRandInt2( iiCons - batch.begin + 1 );
-            btAssert(iSwap >= batch.begin && iSwap < batch.end);
-            bc.m_constraintIndices.swap( iiCons, iSwap );
-        }
-    }
+	btScalar leastSquaresResidual = 0.f;
+	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+
+	for (int iiCons = batchBegin; iiCons < batchEnd; iiCons++)
+	{
+		btScalar totalImpulse = 0;
+		int iContact = contactIndices[iiCons];
+		// apply penetration constraint
+		{
+			const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iContact];
+			btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+			leastSquaresResidual += residual * residual;
+			totalImpulse = solveManifold.m_appliedImpulse;
+		}
+
+		// apply sliding friction
+		if (totalImpulse > 0.0f)
+		{
+			int iBegin = iContact * m_numFrictionDirections;
+			int iEnd = iBegin + m_numFrictionDirections;
+			for (int iFriction = iBegin; iFriction < iEnd; ++iFriction)
+			{
+				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[iFriction];
+				btAssert(solveManifold.m_frictionIndex == iContact);
+
+				solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+				solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
+
+				btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
+				btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
+				btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, solveManifold);
+				leastSquaresResidual += residual * residual;
+			}
+		}
+
+		// apply rolling friction
+		int iFirstRollingFriction = m_rollingFrictionIndexTable[iContact];
+		if (totalImpulse > 0.0f && iFirstRollingFriction >= 0)
+		{
+			int iBegin = iFirstRollingFriction;
+			int iEnd = iBegin + 3;
+			for (int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric)
+			{
+				btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric];
+				if (rollingFrictionConstraint.m_frictionIndex != iContact)
+				{
+					break;
+				}
+				btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
+				if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
+				{
+					rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+				}
+
+				rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+				rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+				btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
+				leastSquaresResidual += residual * residual;
+			}
+		}
+	}
+	return leastSquaresResidual;
 }
 
+void btSequentialImpulseConstraintSolverMt::randomizeBatchedConstraintOrdering(btBatchedConstraints* batchedConstraints)
+{
+	btBatchedConstraints& bc = *batchedConstraints;
+	// randomize ordering of phases
+	for (int ii = 1; ii < bc.m_phaseOrder.size(); ++ii)
+	{
+		int iSwap = btRandInt2(ii + 1);
+		bc.m_phaseOrder.swap(ii, iSwap);
+	}
+
+	// for each batch,
+	for (int iBatch = 0; iBatch < bc.m_batches.size(); ++iBatch)
+	{
+		// randomize ordering of constraints within the batch
+		const btBatchedConstraints::Range& batch = bc.m_batches[iBatch];
+		for (int iiCons = batch.begin; iiCons < batch.end; ++iiCons)
+		{
+			int iSwap = batch.begin + btRandInt2(iiCons - batch.begin + 1);
+			btAssert(iSwap >= batch.begin && iSwap < batch.end);
+			bc.m_constraintIndices.swap(iiCons, iSwap);
+		}
+	}
+}
 
 void btSequentialImpulseConstraintSolverMt::randomizeConstraintOrdering(int iteration, int numIterations)
 {
-    // randomize ordering of joint constraints
-    randomizeBatchedConstraintOrdering( &m_batchedJointConstraints );
-
-    //contact/friction constraints are not solved more than numIterations
-    if ( iteration < numIterations )
-    {
-        randomizeBatchedConstraintOrdering( &m_batchedContactConstraints );
-    }
-}
+	// randomize ordering of joint constraints
+	randomizeBatchedConstraintOrdering(&m_batchedJointConstraints);
 
+	//contact/friction constraints are not solved more than numIterations
+	if (iteration < numIterations)
+	{
+		randomizeBatchedConstraintOrdering(&m_batchedContactConstraints);
+	}
+}
 
 struct JointSolverLoop : public btIParallelSumBody
 {
-    btSequentialImpulseConstraintSolverMt* m_solver;
-    const btBatchedConstraints* m_bc;
-    int m_iteration;
-
-    JointSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, int iteration )
-    {
-        m_solver = solver;
-        m_bc = bc;
-        m_iteration = iteration;
-    }
-    btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE
-    {
-        BT_PROFILE( "JointSolverLoop" );
-        btScalar sum = 0;
-        for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch )
-        {
-            const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ];
-            sum += m_solver->resolveMultipleJointConstraints( m_bc->m_constraintIndices, batch.begin, batch.end, m_iteration );
-        }
-        return sum;
-    }
-};
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
+	int m_iteration;
 
+	JointSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, int iteration)
+	{
+		m_solver = solver;
+		m_bc = bc;
+		m_iteration = iteration;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("JointSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleJointConstraints(m_bc->m_constraintIndices, batch.begin, batch.end, m_iteration);
+		}
+		return sum;
+	}
+};
 
 btScalar btSequentialImpulseConstraintSolverMt::resolveAllJointConstraints(int iteration)
 {
-    BT_PROFILE( "resolveAllJointConstraints" );
-    const btBatchedConstraints& batchedCons = m_batchedJointConstraints;
-    JointSolverLoop loop( this, &batchedCons, iteration );
-    btScalar leastSquaresResidual = 0.f;
-    for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase )
-    {
-        int iPhase = batchedCons.m_phaseOrder[ iiPhase ];
-        const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ];
-        int grainSize = 1;
-        leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop );
-    }
-    return leastSquaresResidual;
+	BT_PROFILE("resolveAllJointConstraints");
+	const btBatchedConstraints& batchedCons = m_batchedJointConstraints;
+	JointSolverLoop loop(this, &batchedCons, iteration);
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+	{
+		int iPhase = batchedCons.m_phaseOrder[iiPhase];
+		const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+		int grainSize = 1;
+		leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+	}
+	return leastSquaresResidual;
 }
 
-
 struct ContactSolverLoop : public btIParallelSumBody
 {
-    btSequentialImpulseConstraintSolverMt* m_solver;
-    const btBatchedConstraints* m_bc;
-
-    ContactSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc )
-    {
-        m_solver = solver;
-        m_bc = bc;
-    }
-    btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE
-    {
-        BT_PROFILE( "ContactSolverLoop" );
-        btScalar sum = 0;
-        for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch )
-        {
-            const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ];
-            sum += m_solver->resolveMultipleContactConstraints( m_bc->m_constraintIndices, batch.begin, batch.end );
-        }
-        return sum;
-    }
-};
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
 
+	ContactSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
+	{
+		m_solver = solver;
+		m_bc = bc;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("ContactSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleContactConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
+		}
+		return sum;
+	}
+};
 
 btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraints()
 {
-    BT_PROFILE( "resolveAllContactConstraints" );
-    const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
-    ContactSolverLoop loop( this, &batchedCons );
-    btScalar leastSquaresResidual = 0.f;
-    for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase )
-    {
-        int iPhase = batchedCons.m_phaseOrder[ iiPhase ];
-        const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ];
-        int grainSize = batchedCons.m_phaseGrainSize[iPhase];
-        leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop );
-    }
-    return leastSquaresResidual;
+	BT_PROFILE("resolveAllContactConstraints");
+	const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+	ContactSolverLoop loop(this, &batchedCons);
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+	{
+		int iPhase = batchedCons.m_phaseOrder[iiPhase];
+		const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+		int grainSize = batchedCons.m_phaseGrainSize[iPhase];
+		leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+	}
+	return leastSquaresResidual;
 }
 
-
 struct ContactFrictionSolverLoop : public btIParallelSumBody
 {
-    btSequentialImpulseConstraintSolverMt* m_solver;
-    const btBatchedConstraints* m_bc;
-
-    ContactFrictionSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc )
-    {
-        m_solver = solver;
-        m_bc = bc;
-    }
-    btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE
-    {
-        BT_PROFILE( "ContactFrictionSolverLoop" );
-        btScalar sum = 0;
-        for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch )
-        {
-            const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ];
-            sum += m_solver->resolveMultipleContactFrictionConstraints( m_bc->m_constraintIndices, batch.begin, batch.end );
-        }
-        return sum;
-    }
-};
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
 
+	ContactFrictionSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
+	{
+		m_solver = solver;
+		m_bc = bc;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("ContactFrictionSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleContactFrictionConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
+		}
+		return sum;
+	}
+};
 
 btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactFrictionConstraints()
 {
-    BT_PROFILE( "resolveAllContactFrictionConstraints" );
-    const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
-    ContactFrictionSolverLoop loop( this, &batchedCons );
-    btScalar leastSquaresResidual = 0.f;
-    for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase )
-    {
-        int iPhase = batchedCons.m_phaseOrder[ iiPhase ];
-        const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ];
-        int grainSize = batchedCons.m_phaseGrainSize[iPhase];
-        leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop );
-    }
-    return leastSquaresResidual;
+	BT_PROFILE("resolveAllContactFrictionConstraints");
+	const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+	ContactFrictionSolverLoop loop(this, &batchedCons);
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+	{
+		int iPhase = batchedCons.m_phaseOrder[iiPhase];
+		const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+		int grainSize = batchedCons.m_phaseGrainSize[iPhase];
+		leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+	}
+	return leastSquaresResidual;
 }
 
-
 struct InterleavedContactSolverLoop : public btIParallelSumBody
 {
-    btSequentialImpulseConstraintSolverMt* m_solver;
-    const btBatchedConstraints* m_bc;
-
-    InterleavedContactSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc )
-    {
-        m_solver = solver;
-        m_bc = bc;
-    }
-    btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE
-    {
-        BT_PROFILE( "InterleavedContactSolverLoop" );
-        btScalar sum = 0;
-        for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch )
-        {
-            const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ];
-            sum += m_solver->resolveMultipleContactConstraintsInterleaved( m_bc->m_constraintIndices, batch.begin, batch.end );
-        }
-        return sum;
-    }
-};
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
 
+	InterleavedContactSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
+	{
+		m_solver = solver;
+		m_bc = bc;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("InterleavedContactSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleContactConstraintsInterleaved(m_bc->m_constraintIndices, batch.begin, batch.end);
+		}
+		return sum;
+	}
+};
 
 btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraintsInterleaved()
 {
-    BT_PROFILE( "resolveAllContactConstraintsInterleaved" );
-    const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
-    InterleavedContactSolverLoop loop( this, &batchedCons );
-    btScalar leastSquaresResidual = 0.f;
-    for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase )
-    {
-        int iPhase = batchedCons.m_phaseOrder[ iiPhase ];
-        const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ];
-        int grainSize = 1;
-        leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop );
-    }
-    return leastSquaresResidual;
+	BT_PROFILE("resolveAllContactConstraintsInterleaved");
+	const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+	InterleavedContactSolverLoop loop(this, &batchedCons);
+	btScalar leastSquaresResidual = 0.f;
+	for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+	{
+		int iPhase = batchedCons.m_phaseOrder[iiPhase];
+		const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+		int grainSize = 1;
+		leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+	}
+	return leastSquaresResidual;
 }
 
-
 struct ContactRollingFrictionSolverLoop : public btIParallelSumBody
 {
-    btSequentialImpulseConstraintSolverMt* m_solver;
-    const btBatchedConstraints* m_bc;
-
-    ContactRollingFrictionSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc )
-    {
-        m_solver = solver;
-        m_bc = bc;
-    }
-    btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE
-    {
-        BT_PROFILE( "ContactFrictionSolverLoop" );
-        btScalar sum = 0;
-        for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch )
-        {
-            const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ];
-            sum += m_solver->resolveMultipleContactRollingFrictionConstraints( m_bc->m_constraintIndices, batch.begin, batch.end );
-        }
-        return sum;
-    }
-};
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btBatchedConstraints* m_bc;
 
+	ContactRollingFrictionSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
+	{
+		m_solver = solver;
+		m_bc = bc;
+	}
+	btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		BT_PROFILE("ContactFrictionSolverLoop");
+		btScalar sum = 0;
+		for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
+		{
+			const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
+			sum += m_solver->resolveMultipleContactRollingFrictionConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
+		}
+		return sum;
+	}
+};
 
 btScalar btSequentialImpulseConstraintSolverMt::resolveAllRollingFrictionConstraints()
 {
-    BT_PROFILE( "resolveAllRollingFrictionConstraints" );
-    btScalar leastSquaresResidual = 0.f;
-    //
-    // We do not generate batches for rolling friction constraints. We assume that
-    // one of two cases is true:
-    //
-    //  1. either most bodies in the simulation have rolling friction, in which case we can use the
-    //     batches for contacts and use a lookup table to translate contact indices to rolling friction
-    //     (ignoring any contact indices that don't map to a rolling friction constraint). As long as
-    //     most contacts have a corresponding rolling friction constraint, this should parallelize well.
-    //
-    //  -OR-
-    //
-    //  2. few bodies in the simulation have rolling friction, so it is not worth trying to use the
-    //     batches from contacts as most of the contacts won't have corresponding rolling friction
-    //     constraints and most threads would end up doing very little work. Most of the time would
-    //     go to threading overhead, so we don't bother with threading.
-    //
-    int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-    if (numRollingFrictionPoolConstraints >= m_tmpSolverContactConstraintPool.size())
-    {
-        // use batching if there are many rolling friction constraints
-        const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
-        ContactRollingFrictionSolverLoop loop( this, &batchedCons );
-        btScalar leastSquaresResidual = 0.f;
-        for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase )
-        {
-            int iPhase = batchedCons.m_phaseOrder[ iiPhase ];
-            const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ];
-            int grainSize = 1;
-            leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop );
-        }
-    }
-    else
-    {
-        // no batching, also ignores SOLVER_RANDMIZE_ORDER
-        for ( int j = 0; j < numRollingFrictionPoolConstraints; j++ )
-        {
-            btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ j ];
-            if ( rollingFrictionConstraint.m_frictionIndex >= 0 )
-            {
-                btScalar totalImpulse = m_tmpSolverContactConstraintPool[ rollingFrictionConstraint.m_frictionIndex ].m_appliedImpulse;
-                if ( totalImpulse > 0.0f )
-                {
-                    btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
-                    if ( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction )
-                        rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
-
-                    rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
-                    rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
-
-                    btScalar residual = resolveSingleConstraintRowGeneric( m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdA ], m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdB ], rollingFrictionConstraint );
-                    leastSquaresResidual += residual*residual;
-                }
-            }
-        }
-    }
-    return leastSquaresResidual;
+	BT_PROFILE("resolveAllRollingFrictionConstraints");
+	btScalar leastSquaresResidual = 0.f;
+	//
+	// We do not generate batches for rolling friction constraints. We assume that
+	// one of two cases is true:
+	//
+	//  1. either most bodies in the simulation have rolling friction, in which case we can use the
+	//     batches for contacts and use a lookup table to translate contact indices to rolling friction
+	//     (ignoring any contact indices that don't map to a rolling friction constraint). As long as
+	//     most contacts have a corresponding rolling friction constraint, this should parallelize well.
+	//
+	//  -OR-
+	//
+	//  2. few bodies in the simulation have rolling friction, so it is not worth trying to use the
+	//     batches from contacts as most of the contacts won't have corresponding rolling friction
+	//     constraints and most threads would end up doing very little work. Most of the time would
+	//     go to threading overhead, so we don't bother with threading.
+	//
+	int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+	if (numRollingFrictionPoolConstraints >= m_tmpSolverContactConstraintPool.size())
+	{
+		// use batching if there are many rolling friction constraints
+		const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
+		ContactRollingFrictionSolverLoop loop(this, &batchedCons);
+		btScalar leastSquaresResidual = 0.f;
+		for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
+		{
+			int iPhase = batchedCons.m_phaseOrder[iiPhase];
+			const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
+			int grainSize = 1;
+			leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
+		}
+	}
+	else
+	{
+		// no batching, also ignores SOLVER_RANDMIZE_ORDER
+		for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
+		{
+			btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+			if (rollingFrictionConstraint.m_frictionIndex >= 0)
+			{
+				btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+				if (totalImpulse > 0.0f)
+				{
+					btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
+					if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
+						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+
+					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+					btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
+					leastSquaresResidual += residual * residual;
+				}
+			}
+		}
+	}
+	return leastSquaresResidual;
 }
 
-
-void btSequentialImpulseConstraintSolverMt::internalWriteBackContacts( int iBegin, int iEnd, const btContactSolverInfo& infoGlobal )
+void btSequentialImpulseConstraintSolverMt::internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
 {
-    BT_PROFILE("internalWriteBackContacts");
-    writeBackContacts(iBegin, iEnd, infoGlobal);
-    //for ( int iContact = iBegin; iContact < iEnd; ++iContact)
-    //{
-    //    const btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[ iContact ];
-    //    btManifoldPoint* pt = (btManifoldPoint*) contactConstraint.m_originalContactPoint;
-    //    btAssert( pt );
-    //    pt->m_appliedImpulse = contactConstraint.m_appliedImpulse;
-    //    pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex ].m_appliedImpulse;
-    //    if ( m_numFrictionDirections == 2 )
-    //    {
-    //        pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex + 1 ].m_appliedImpulse;
-    //    }
-    //}
+	BT_PROFILE("internalWriteBackContacts");
+	writeBackContacts(iBegin, iEnd, infoGlobal);
+	//for ( int iContact = iBegin; iContact < iEnd; ++iContact)
+	//{
+	//    const btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[ iContact ];
+	//    btManifoldPoint* pt = (btManifoldPoint*) contactConstraint.m_originalContactPoint;
+	//    btAssert( pt );
+	//    pt->m_appliedImpulse = contactConstraint.m_appliedImpulse;
+	//    pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex ].m_appliedImpulse;
+	//    if ( m_numFrictionDirections == 2 )
+	//    {
+	//        pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex + 1 ].m_appliedImpulse;
+	//    }
+	//}
 }
 
-
-void btSequentialImpulseConstraintSolverMt::internalWriteBackJoints( int iBegin, int iEnd, const btContactSolverInfo& infoGlobal )
+void btSequentialImpulseConstraintSolverMt::internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
 {
 	BT_PROFILE("internalWriteBackJoints");
-    writeBackJoints(iBegin, iEnd, infoGlobal);
+	writeBackJoints(iBegin, iEnd, infoGlobal);
 }
 
-
-void btSequentialImpulseConstraintSolverMt::internalWriteBackBodies( int iBegin, int iEnd, const btContactSolverInfo& infoGlobal )
+void btSequentialImpulseConstraintSolverMt::internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
 {
 	BT_PROFILE("internalWriteBackBodies");
-    writeBackBodies( iBegin, iEnd, infoGlobal );
+	writeBackBodies(iBegin, iEnd, infoGlobal);
 }
 
-
 struct WriteContactPointsLoop : public btIParallelForBody
 {
-    btSequentialImpulseConstraintSolverMt* m_solver;
-    const btContactSolverInfo* m_infoGlobal;
-
-    WriteContactPointsLoop( btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal )
-    {
-        m_solver = solver;
-        m_infoGlobal = &infoGlobal;
-    }
-    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
-    {
-        m_solver->internalWriteBackContacts( iBegin, iEnd, *m_infoGlobal );
-    }
-};
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btContactSolverInfo* m_infoGlobal;
 
+	WriteContactPointsLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal)
+	{
+		m_solver = solver;
+		m_infoGlobal = &infoGlobal;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalWriteBackContacts(iBegin, iEnd, *m_infoGlobal);
+	}
+};
 
 struct WriteJointsLoop : public btIParallelForBody
 {
-    btSequentialImpulseConstraintSolverMt* m_solver;
-    const btContactSolverInfo* m_infoGlobal;
-
-    WriteJointsLoop( btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal )
-    {
-        m_solver = solver;
-        m_infoGlobal = &infoGlobal;
-    }
-    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
-    {
-        m_solver->internalWriteBackJoints( iBegin, iEnd, *m_infoGlobal );
-    }
-};
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btContactSolverInfo* m_infoGlobal;
 
+	WriteJointsLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal)
+	{
+		m_solver = solver;
+		m_infoGlobal = &infoGlobal;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalWriteBackJoints(iBegin, iEnd, *m_infoGlobal);
+	}
+};
 
 struct WriteBodiesLoop : public btIParallelForBody
 {
-    btSequentialImpulseConstraintSolverMt* m_solver;
-    const btContactSolverInfo* m_infoGlobal;
-
-    WriteBodiesLoop( btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal )
-    {
-        m_solver = solver;
-        m_infoGlobal = &infoGlobal;
-    }
-    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
-    {
-        m_solver->internalWriteBackBodies( iBegin, iEnd, *m_infoGlobal );
-    }
-};
+	btSequentialImpulseConstraintSolverMt* m_solver;
+	const btContactSolverInfo* m_infoGlobal;
 
+	WriteBodiesLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal)
+	{
+		m_solver = solver;
+		m_infoGlobal = &infoGlobal;
+	}
+	void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
+	{
+		m_solver->internalWriteBackBodies(iBegin, iEnd, *m_infoGlobal);
+	}
+};
 
 btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
 {
 	BT_PROFILE("solveGroupCacheFriendlyFinish");
 
 	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-    {
-        WriteContactPointsLoop loop( this, infoGlobal );
-        int grainSize = 500;
-        btParallelFor( 0, m_tmpSolverContactConstraintPool.size(), grainSize, loop );
-    }
-
-    {
-        WriteJointsLoop loop( this, infoGlobal );
-        int grainSize = 400;
-        btParallelFor( 0, m_tmpSolverNonContactConstraintPool.size(), grainSize, loop );
-    }
-    {
-        WriteBodiesLoop loop( this, infoGlobal );
-        int grainSize = 100;
-        btParallelFor( 0, m_tmpSolverBodyPool.size(), grainSize, loop );
-    }
+	{
+		WriteContactPointsLoop loop(this, infoGlobal);
+		int grainSize = 500;
+		btParallelFor(0, m_tmpSolverContactConstraintPool.size(), grainSize, loop);
+	}
+
+	{
+		WriteJointsLoop loop(this, infoGlobal);
+		int grainSize = 400;
+		btParallelFor(0, m_tmpSolverNonContactConstraintPool.size(), grainSize, loop);
+	}
+	{
+		WriteBodiesLoop loop(this, infoGlobal);
+		int grainSize = 100;
+		btParallelFor(0, m_tmpSolverBodyPool.size(), grainSize, loop);
+	}
 
 	m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
 	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
@@ -1618,4 +1552,3 @@ btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlyFinish(bt
 	m_tmpSolverBodyPool.resizeNoInitialize(0);
 	return 0.f;
 }
-