Update Bullet to the latest commit 126b676

20 files changed, 2001 insertions, 2399 deletions

diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h
index 7a12257b33..049c9116fd 100644
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h
@@ -18,7 +18,7 @@ subject to the following restrictions:
 
 #include "Bullet3Common/b3Scalar.h"
 
-enum	b3SolverMode
+enum b3SolverMode
 {
 	B3_SOLVER_RANDMIZE_ORDER = 1,
 	B3_SOLVER_FRICTION_SEPARATE = 2,
@@ -34,45 +34,38 @@ enum	b3SolverMode
 
 struct b3ContactSolverInfoData
 {
-	
-
-	b3Scalar	m_tau;
-	b3Scalar	m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
-	b3Scalar	m_friction;
-	b3Scalar	m_timeStep;
-	b3Scalar	m_restitution;
-	int		m_numIterations;
-	b3Scalar	m_maxErrorReduction;
-	b3Scalar	m_sor;
-	b3Scalar	m_erp;//used as Baumgarte factor
-	b3Scalar	m_erp2;//used in Split Impulse
-	b3Scalar	m_globalCfm;//constraint force mixing
-	int			m_splitImpulse;
-	b3Scalar	m_splitImpulsePenetrationThreshold;
-	b3Scalar	m_splitImpulseTurnErp;
-	b3Scalar	m_linearSlop;
-	b3Scalar	m_warmstartingFactor;
-
-	int			m_solverMode;
-	int	m_restingContactRestitutionThreshold;
-	int			m_minimumSolverBatchSize;
-	b3Scalar	m_maxGyroscopicForce;
-	b3Scalar	m_singleAxisRollingFrictionThreshold;
-
-
+	b3Scalar m_tau;
+	b3Scalar m_damping;  //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	b3Scalar m_friction;
+	b3Scalar m_timeStep;
+	b3Scalar m_restitution;
+	int m_numIterations;
+	b3Scalar m_maxErrorReduction;
+	b3Scalar m_sor;
+	b3Scalar m_erp;        //used as Baumgarte factor
+	b3Scalar m_erp2;       //used in Split Impulse
+	b3Scalar m_globalCfm;  //constraint force mixing
+	int m_splitImpulse;
+	b3Scalar m_splitImpulsePenetrationThreshold;
+	b3Scalar m_splitImpulseTurnErp;
+	b3Scalar m_linearSlop;
+	b3Scalar m_warmstartingFactor;
+
+	int m_solverMode;
+	int m_restingContactRestitutionThreshold;
+	int m_minimumSolverBatchSize;
+	b3Scalar m_maxGyroscopicForce;
+	b3Scalar m_singleAxisRollingFrictionThreshold;
 };
 
 struct b3ContactSolverInfo : public b3ContactSolverInfoData
 {
-
-	
-
 	inline b3ContactSolverInfo()
 	{
 		m_tau = b3Scalar(0.6);
 		m_damping = b3Scalar(1.0);
 		m_friction = b3Scalar(0.3);
-		m_timeStep = b3Scalar(1.f/60.f);
+		m_timeStep = b3Scalar(1.f / 60.f);
 		m_restitution = b3Scalar(0.);
 		m_maxErrorReduction = b3Scalar(20.);
 		m_numIterations = 10;
@@ -84,76 +77,73 @@ struct b3ContactSolverInfo : public b3ContactSolverInfoData
 		m_splitImpulsePenetrationThreshold = -.04f;
 		m_splitImpulseTurnErp = 0.1f;
 		m_linearSlop = b3Scalar(0.0);
-		m_warmstartingFactor=b3Scalar(0.85);
+		m_warmstartingFactor = b3Scalar(0.85);
 		//m_solverMode =  B3_SOLVER_USE_WARMSTARTING |  B3_SOLVER_SIMD | B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | B3_SOLVER_RANDMIZE_ORDER;
-		m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD;// | B3_SOLVER_RANDMIZE_ORDER;
-		m_restingContactRestitutionThreshold = 2;//unused as of 2.81
-		m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
-		m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their B3_ENABLE_GYROPSCOPIC_FORCE flag set (using b3RigidBody::setFlag)
-		m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
+		m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD;  // | B3_SOLVER_RANDMIZE_ORDER;
+		m_restingContactRestitutionThreshold = 2;                    //unused as of 2.81
+		m_minimumSolverBatchSize = 128;                              //try to combine islands until the amount of constraints reaches this limit
+		m_maxGyroscopicForce = 100.f;                                ///only used to clamp forces for bodies that have their B3_ENABLE_GYROPSCOPIC_FORCE flag set (using b3RigidBody::setFlag)
+		m_singleAxisRollingFrictionThreshold = 1e30f;                ///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
 	}
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct b3ContactSolverInfoDoubleData
 {
-	double		m_tau;
-	double		m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
-	double		m_friction;
-	double		m_timeStep;
-	double		m_restitution;
-	double		m_maxErrorReduction;
-	double		m_sor;
-	double		m_erp;//used as Baumgarte factor
-	double		m_erp2;//used in Split Impulse
-	double		m_globalCfm;//constraint force mixing
-	double		m_splitImpulsePenetrationThreshold;
-	double		m_splitImpulseTurnErp;
-	double		m_linearSlop;
-	double		m_warmstartingFactor;
-	double		m_maxGyroscopicForce;
-	double		m_singleAxisRollingFrictionThreshold;
-
-	int			m_numIterations;
-	int			m_solverMode;
-	int			m_restingContactRestitutionThreshold;
-	int			m_minimumSolverBatchSize;
-	int			m_splitImpulse;
-	char		m_padding[4];
-
+	double m_tau;
+	double m_damping;  //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	double m_friction;
+	double m_timeStep;
+	double m_restitution;
+	double m_maxErrorReduction;
+	double m_sor;
+	double m_erp;        //used as Baumgarte factor
+	double m_erp2;       //used in Split Impulse
+	double m_globalCfm;  //constraint force mixing
+	double m_splitImpulsePenetrationThreshold;
+	double m_splitImpulseTurnErp;
+	double m_linearSlop;
+	double m_warmstartingFactor;
+	double m_maxGyroscopicForce;
+	double m_singleAxisRollingFrictionThreshold;
+
+	int m_numIterations;
+	int m_solverMode;
+	int m_restingContactRestitutionThreshold;
+	int m_minimumSolverBatchSize;
+	int m_splitImpulse;
+	char m_padding[4];
 };
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct b3ContactSolverInfoFloatData
 {
-	float		m_tau;
-	float		m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
-	float		m_friction;
-	float		m_timeStep;
-
-	float		m_restitution;
-	float		m_maxErrorReduction;
-	float		m_sor;
-	float		m_erp;//used as Baumgarte factor
-
-	float		m_erp2;//used in Split Impulse
-	float		m_globalCfm;//constraint force mixing
-	float		m_splitImpulsePenetrationThreshold;
-	float		m_splitImpulseTurnErp;
-
-	float		m_linearSlop;
-	float		m_warmstartingFactor;
-	float		m_maxGyroscopicForce;
-	float		m_singleAxisRollingFrictionThreshold;
-
-	int			m_numIterations;
-	int			m_solverMode;
-	int			m_restingContactRestitutionThreshold;
-	int			m_minimumSolverBatchSize;
-
-	int			m_splitImpulse;
-	char		m_padding[4];
+	float m_tau;
+	float m_damping;  //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	float m_friction;
+	float m_timeStep;
+
+	float m_restitution;
+	float m_maxErrorReduction;
+	float m_sor;
+	float m_erp;  //used as Baumgarte factor
+
+	float m_erp2;       //used in Split Impulse
+	float m_globalCfm;  //constraint force mixing
+	float m_splitImpulsePenetrationThreshold;
+	float m_splitImpulseTurnErp;
+
+	float m_linearSlop;
+	float m_warmstartingFactor;
+	float m_maxGyroscopicForce;
+	float m_singleAxisRollingFrictionThreshold;
+
+	int m_numIterations;
+	int m_solverMode;
+	int m_restingContactRestitutionThreshold;
+	int m_minimumSolverBatchSize;
+
+	int m_splitImpulse;
+	char m_padding[4];
 };
 
-
-
-#endif //B3_CONTACT_SOLVER_INFO
+#endif  //B3_CONTACT_SOLVER_INFO
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp
index 5e11e74935..ace4b18388 100644
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp
@@ -4,105 +4,100 @@
 #include "Bullet3Common/b3TransformUtil.h"
 #include <new>
 
-
-b3FixedConstraint::b3FixedConstraint(int rbA,int rbB, const b3Transform& frameInA,const b3Transform& frameInB)
-:b3TypedConstraint(B3_FIXED_CONSTRAINT_TYPE,rbA,rbB)
+b3FixedConstraint::b3FixedConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB)
+	: b3TypedConstraint(B3_FIXED_CONSTRAINT_TYPE, rbA, rbB)
 {
 	m_pivotInA = frameInA.getOrigin();
 	m_pivotInB = frameInB.getOrigin();
-	m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
-
+	m_relTargetAB = frameInA.getRotation() * frameInB.getRotation().inverse();
 }
 
-b3FixedConstraint::~b3FixedConstraint ()
+b3FixedConstraint::~b3FixedConstraint()
 {
 }
 
-	
-void b3FixedConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
+void b3FixedConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
 {
 	info->m_numConstraintRows = 6;
 	info->nub = 6;
 }
 
-void b3FixedConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
+void b3FixedConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
 {
 	//fix the 3 linear degrees of freedom
 
 	const b3Vector3& worldPosA = bodies[m_rbA].m_pos;
 	const b3Quaternion& worldOrnA = bodies[m_rbA].m_quat;
-	const b3Vector3& worldPosB= bodies[m_rbB].m_pos;
+	const b3Vector3& worldPosB = bodies[m_rbB].m_pos;
 	const b3Quaternion& worldOrnB = bodies[m_rbB].m_quat;
 
 	info->m_J1linearAxis[0] = 1;
-	info->m_J1linearAxis[info->rowskip+1] = 1;
-	info->m_J1linearAxis[2*info->rowskip+2] = 1;
+	info->m_J1linearAxis[info->rowskip + 1] = 1;
+	info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
 
-	b3Vector3 a1 = b3QuatRotate(worldOrnA,m_pivotInA);
+	b3Vector3 a1 = b3QuatRotate(worldOrnA, m_pivotInA);
 	{
 		b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis);
-		b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip);
-		b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip);
+		b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip);
+		b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip);
 		b3Vector3 a1neg = -a1;
-		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
-    
+
 	if (info->m_J2linearAxis)
 	{
 		info->m_J2linearAxis[0] = -1;
-		info->m_J2linearAxis[info->rowskip+1] = -1;
-		info->m_J2linearAxis[2*info->rowskip+2] = -1;
+		info->m_J2linearAxis[info->rowskip + 1] = -1;
+		info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
 	}
-	
-	b3Vector3 a2 = b3QuatRotate(worldOrnB,m_pivotInB);
-   
+
+	b3Vector3 a2 = b3QuatRotate(worldOrnB, m_pivotInB);
+
 	{
-	//	b3Vector3 a2n = -a2;
+		//	b3Vector3 a2n = -a2;
 		b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis);
-		b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip);
-		b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip);
-		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip);
+		b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
 
-    // set right hand side for the linear dofs
+	// set right hand side for the linear dofs
 	b3Scalar k = info->fps * info->erp;
-	b3Vector3 linearError = k*(a2+worldPosB-a1-worldPosA);
-    int j;
-	for (j=0; j<3; j++)
-    {
-        info->m_constraintError[j*info->rowskip] = linearError[j];
+	b3Vector3 linearError = k * (a2 + worldPosB - a1 - worldPosA);
+	int j;
+	for (j = 0; j < 3; j++)
+	{
+		info->m_constraintError[j * info->rowskip] = linearError[j];
 		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
-    }
+	}
 
-		//fix the 3 angular degrees of freedom
+	//fix the 3 angular degrees of freedom
 
 	int start_row = 3;
 	int s = info->rowskip;
-    int start_index = start_row * s;
+	int start_index = start_row * s;
 
-    // 3 rows to make body rotations equal
+	// 3 rows to make body rotations equal
 	info->m_J1angularAxis[start_index] = 1;
-    info->m_J1angularAxis[start_index + s + 1] = 1;
-    info->m_J1angularAxis[start_index + s*2+2] = 1;
-    if ( info->m_J2angularAxis)
-    {
-        info->m_J2angularAxis[start_index] = -1;
-        info->m_J2angularAxis[start_index + s+1] = -1;
-        info->m_J2angularAxis[start_index + s*2+2] = -1;
-    }
-
+	info->m_J1angularAxis[start_index + s + 1] = 1;
+	info->m_J1angularAxis[start_index + s * 2 + 2] = 1;
+	if (info->m_J2angularAxis)
+	{
+		info->m_J2angularAxis[start_index] = -1;
+		info->m_J2angularAxis[start_index + s + 1] = -1;
+		info->m_J2angularAxis[start_index + s * 2 + 2] = -1;
+	}
 
-    // set right hand side for the angular dofs
+	// set right hand side for the angular dofs
 
 	b3Vector3 diff;
 	b3Scalar angle;
-	b3Quaternion qrelCur = worldOrnA *worldOrnB.inverse();
-
-	b3TransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle);
-	diff*=-angle;
-	for (j=0; j<3; j++)
-    {
-        info->m_constraintError[(3+j)*info->rowskip] = k * diff[j];
-    }
+	b3Quaternion qrelCur = worldOrnA * worldOrnB.inverse();
 
+	b3TransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB, qrelCur, diff, angle);
+	diff *= -angle;
+	for (j = 0; j < 3; j++)
+	{
+		info->m_constraintError[(3 + j) * info->rowskip] = k * diff[j];
+	}
 }
\ No newline at end of file
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h
index e884a82912..64809666e4 100644
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h
@@ -4,32 +4,31 @@
 
 #include "b3TypedConstraint.h"
 
-B3_ATTRIBUTE_ALIGNED16(class) b3FixedConstraint : public b3TypedConstraint
+B3_ATTRIBUTE_ALIGNED16(class)
+b3FixedConstraint : public b3TypedConstraint
 {
 	b3Vector3 m_pivotInA;
 	b3Vector3 m_pivotInB;
 	b3Quaternion m_relTargetAB;
 
 public:
-	b3FixedConstraint(int  rbA,int rbB, const b3Transform& frameInA,const b3Transform& frameInB);
-	
+	b3FixedConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB);
+
 	virtual ~b3FixedConstraint();
 
-	
-	virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
+	virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
 
-	virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies);
+	virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
 
-	virtual	void	setParam(int num, b3Scalar value, int axis = -1)
+	virtual void setParam(int num, b3Scalar value, int axis = -1)
 	{
 		b3Assert(0);
 	}
-	virtual	b3Scalar getParam(int num, int axis = -1) const
+	virtual b3Scalar getParam(int num, int axis = -1) const
 	{
 		b3Assert(0);
 		return 0.f;
 	}
-
 };
 
-#endif //B3_FIXED_CONSTRAINT_H
+#endif  //B3_FIXED_CONSTRAINT_H
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp
index 3ae2922e58..0d5bb2014b 100644
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp
@@ -26,69 +26,48 @@ http://gimpact.sf.net
 #include "Bullet3Common/b3TransformUtil.h"
 #include <new>
 
-
-
 #define D6_USE_OBSOLETE_METHOD false
 #define D6_USE_FRAME_OFFSET true
 
-
-
-
-
-
-b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA,int  rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies)
-: b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB)
-, m_frameInA(frameInA)
-, m_frameInB(frameInB),
-m_useLinearReferenceFrameA(useLinearReferenceFrameA),
-m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
-m_flags(0)
+b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies)
+	: b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), m_flags(0)
 {
 	calculateTransforms(bodies);
 }
 
-
-
-
-
-
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
 
-
-
 b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index);
 b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index)
 {
-	int i = index%3;
-	int j = index/3;
+	int i = index % 3;
+	int j = index / 3;
 	return mat[i][j];
 }
 
-
-
 ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
-bool	matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz);
-bool	matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz)
+bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz);
+bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz)
 {
 	//	// rot =  cy*cz          -cy*sz           sy
 	//	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
 	//	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
 	//
 
-	b3Scalar fi = btGetMatrixElem(mat,2);
+	b3Scalar fi = btGetMatrixElem(mat, 2);
 	if (fi < b3Scalar(1.0f))
 	{
 		if (fi > b3Scalar(-1.0f))
 		{
-			xyz[0] = b3Atan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
-			xyz[1] = b3Asin(btGetMatrixElem(mat,2));
-			xyz[2] = b3Atan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+			xyz[0] = b3Atan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
+			xyz[1] = b3Asin(btGetMatrixElem(mat, 2));
+			xyz[2] = b3Atan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
 			return true;
 		}
 		else
 		{
 			// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
-			xyz[0] = -b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+			xyz[0] = -b3Atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
 			xyz[1] = -B3_HALF_PI;
 			xyz[2] = b3Scalar(0.0);
 			return false;
@@ -97,7 +76,7 @@ bool	matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz)
 	else
 	{
 		// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
-		xyz[0] = b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+		xyz[0] = b3Atan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
 		xyz[1] = B3_HALF_PI;
 		xyz[2] = 0.0;
 	}
@@ -108,85 +87,75 @@ bool	matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz)
 
 int b3RotationalLimitMotor::testLimitValue(b3Scalar test_value)
 {
-	if(m_loLimit>m_hiLimit)
+	if (m_loLimit > m_hiLimit)
 	{
-		m_currentLimit = 0;//Free from violation
+		m_currentLimit = 0;  //Free from violation
 		return 0;
 	}
 	if (test_value < m_loLimit)
 	{
-		m_currentLimit = 1;//low limit violation
-		m_currentLimitError =  test_value - m_loLimit;
-		if(m_currentLimitError>B3_PI) 
-			m_currentLimitError-=B3_2_PI;
-		else if(m_currentLimitError<-B3_PI) 
-			m_currentLimitError+=B3_2_PI;
+		m_currentLimit = 1;  //low limit violation
+		m_currentLimitError = test_value - m_loLimit;
+		if (m_currentLimitError > B3_PI)
+			m_currentLimitError -= B3_2_PI;
+		else if (m_currentLimitError < -B3_PI)
+			m_currentLimitError += B3_2_PI;
 		return 1;
 	}
-	else if (test_value> m_hiLimit)
+	else if (test_value > m_hiLimit)
 	{
-		m_currentLimit = 2;//High limit violation
+		m_currentLimit = 2;  //High limit violation
 		m_currentLimitError = test_value - m_hiLimit;
-		if(m_currentLimitError>B3_PI) 
-			m_currentLimitError-=B3_2_PI;
-		else if(m_currentLimitError<-B3_PI) 
-			m_currentLimitError+=B3_2_PI;
+		if (m_currentLimitError > B3_PI)
+			m_currentLimitError -= B3_2_PI;
+		else if (m_currentLimitError < -B3_PI)
+			m_currentLimitError += B3_2_PI;
 		return 2;
 	};
 
-	m_currentLimit = 0;//Free from violation
+	m_currentLimit = 0;  //Free from violation
 	return 0;
-
 }
 
-
-
-
 //////////////////////////// End b3RotationalLimitMotor ////////////////////////////////////
 
-
-
-
 //////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
 
-
 int b3TranslationalLimitMotor::testLimitValue(int limitIndex, b3Scalar test_value)
 {
 	b3Scalar loLimit = m_lowerLimit[limitIndex];
 	b3Scalar hiLimit = m_upperLimit[limitIndex];
-	if(loLimit > hiLimit)
+	if (loLimit > hiLimit)
 	{
-		m_currentLimit[limitIndex] = 0;//Free from violation
+		m_currentLimit[limitIndex] = 0;  //Free from violation
 		m_currentLimitError[limitIndex] = b3Scalar(0.f);
 		return 0;
 	}
 
 	if (test_value < loLimit)
 	{
-		m_currentLimit[limitIndex] = 2;//low limit violation
-		m_currentLimitError[limitIndex] =  test_value - loLimit;
+		m_currentLimit[limitIndex] = 2;  //low limit violation
+		m_currentLimitError[limitIndex] = test_value - loLimit;
 		return 2;
 	}
-	else if (test_value> hiLimit)
+	else if (test_value > hiLimit)
 	{
-		m_currentLimit[limitIndex] = 1;//High limit violation
+		m_currentLimit[limitIndex] = 1;  //High limit violation
 		m_currentLimitError[limitIndex] = test_value - hiLimit;
 		return 1;
 	};
 
-	m_currentLimit[limitIndex] = 0;//Free from violation
+	m_currentLimit[limitIndex] = 0;  //Free from violation
 	m_currentLimitError[limitIndex] = b3Scalar(0.f);
 	return 0;
 }
 
-
-
 //////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
 
 void b3Generic6DofConstraint::calculateAngleInfo()
 {
-	b3Matrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
-	matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
+	b3Matrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis();
+	matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff);
 	// in euler angle mode we do not actually constrain the angular velocity
 	// along the axes axis[0] and axis[2] (although we do use axis[1]) :
 	//
@@ -211,12 +180,11 @@ void b3Generic6DofConstraint::calculateAngleInfo()
 	m_calculatedAxis[0].normalize();
 	m_calculatedAxis[1].normalize();
 	m_calculatedAxis[2].normalize();
-
 }
 
 static b3Transform getCenterOfMassTransform(const b3RigidBodyData& body)
 {
-	b3Transform tr(body.m_quat,body.m_pos);
+	b3Transform tr(body.m_quat, body.m_pos);
 	return tr;
 }
 
@@ -226,26 +194,26 @@ void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyData* bodies)
 	b3Transform transB;
 	transA = getCenterOfMassTransform(bodies[m_rbA]);
 	transB = getCenterOfMassTransform(bodies[m_rbB]);
-	calculateTransforms(transA,transB,bodies);
+	calculateTransforms(transA, transB, bodies);
 }
 
-void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies)
 {
 	m_calculatedTransformA = transA * m_frameInA;
 	m_calculatedTransformB = transB * m_frameInB;
 	calculateLinearInfo();
 	calculateAngleInfo();
-	if(m_useOffsetForConstraintFrame)
-	{	//  get weight factors depending on masses
+	if (m_useOffsetForConstraintFrame)
+	{  //  get weight factors depending on masses
 		b3Scalar miA = bodies[m_rbA].m_invMass;
 		b3Scalar miB = bodies[m_rbB].m_invMass;
 		m_hasStaticBody = (miA < B3_EPSILON) || (miB < B3_EPSILON);
 		b3Scalar miS = miA + miB;
-		if(miS > b3Scalar(0.f))
+		if (miS > b3Scalar(0.f))
 		{
 			m_factA = miB / miS;
 		}
-		else 
+		else
 		{
 			m_factA = b3Scalar(0.5f);
 		}
@@ -253,12 +221,6 @@ void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA,cons
 	}
 }
 
-
-
-
-
-
-
 bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
 {
 	b3Scalar angle = m_calculatedAxisAngleDiff[axis_index];
@@ -269,48 +231,43 @@ bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
 	return m_angularLimits[axis_index].needApplyTorques();
 }
 
-
-
-
-void b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
 {
 	//prepare constraint
-	calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]),getCenterOfMassTransform(bodies[m_rbB]),bodies);
+	calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]), getCenterOfMassTransform(bodies[m_rbB]), bodies);
 	info->m_numConstraintRows = 0;
 	info->nub = 6;
 	int i;
 	//test linear limits
-	for(i = 0; i < 3; i++)
+	for (i = 0; i < 3; i++)
 	{
-		if(m_linearLimits.needApplyForce(i))
+		if (m_linearLimits.needApplyForce(i))
 		{
 			info->m_numConstraintRows++;
 			info->nub--;
 		}
 	}
 	//test angular limits
-	for (i=0;i<3 ;i++ )
+	for (i = 0; i < 3; i++)
 	{
-		if(testAngularLimitMotor(i))
+		if (testAngularLimitMotor(i))
 		{
 			info->m_numConstraintRows++;
 			info->nub--;
 		}
 	}
-//	printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows);
+	//	printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows);
 }
 
-void b3Generic6DofConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
 {
 	//pre-allocate all 6
 	info->m_numConstraintRows = 6;
 	info->nub = 0;
 }
 
-
-void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
 {
-
 	b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]);
 	b3Transform transB = getCenterOfMassTransform(bodies[m_rbB]);
 	const b3Vector3& linVelA = bodies[m_rbA].m_linVel;
@@ -318,136 +275,124 @@ void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBod
 	const b3Vector3& angVelA = bodies[m_rbA].m_angVel;
 	const b3Vector3& angVelB = bodies[m_rbB].m_angVel;
 
-	if(m_useOffsetForConstraintFrame)
-	{ // for stability better to solve angular limits first
-		int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
-		setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
+	if (m_useOffsetForConstraintFrame)
+	{  // for stability better to solve angular limits first
+		int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 	}
 	else
-	{ // leave old version for compatibility
-		int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
-		setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
+	{  // leave old version for compatibility
+		int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 	}
-
 }
 
-
-void b3Generic6DofConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies)
 {
-	
 	//prepare constraint
-	calculateTransforms(transA,transB,bodies);
+	calculateTransforms(transA, transB, bodies);
 
 	int i;
-	for (i=0;i<3 ;i++ )
+	for (i = 0; i < 3; i++)
 	{
 		testAngularLimitMotor(i);
 	}
 
-	if(m_useOffsetForConstraintFrame)
-	{ // for stability better to solve angular limits first
-		int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
-		setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
+	if (m_useOffsetForConstraintFrame)
+	{  // for stability better to solve angular limits first
+		int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 	}
 	else
-	{ // leave old version for compatibility
-		int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
-		setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
+	{  // leave old version for compatibility
+		int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+		setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
 	}
 }
 
-
-
-int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB)
+int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB)
 {
-//	int row = 0;
+	//	int row = 0;
 	//solve linear limits
 	b3RotationalLimitMotor limot;
-	for (int i=0;i<3 ;i++ )
+	for (int i = 0; i < 3; i++)
 	{
-		if(m_linearLimits.needApplyForce(i))
-		{ // re-use rotational motor code
+		if (m_linearLimits.needApplyForce(i))
+		{  // re-use rotational motor code
 			limot.m_bounce = b3Scalar(0.f);
 			limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
 			limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
-			limot.m_currentLimitError  = m_linearLimits.m_currentLimitError[i];
-			limot.m_damping  = m_linearLimits.m_damping;
-			limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
-			limot.m_hiLimit  = m_linearLimits.m_upperLimit[i];
-			limot.m_limitSoftness  = m_linearLimits.m_limitSoftness;
-			limot.m_loLimit  = m_linearLimits.m_lowerLimit[i];
-			limot.m_maxLimitForce  = b3Scalar(0.f);
-			limot.m_maxMotorForce  = m_linearLimits.m_maxMotorForce[i];
-			limot.m_targetVelocity  = m_linearLimits.m_targetVelocity[i];
+			limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
+			limot.m_damping = m_linearLimits.m_damping;
+			limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
+			limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
+			limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
+			limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
+			limot.m_maxLimitForce = b3Scalar(0.f);
+			limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
+			limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
 			b3Vector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
 			int flags = m_flags >> (i * B3_6DOF_FLAGS_AXIS_SHIFT);
-			limot.m_normalCFM	= (flags & B3_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
-			limot.m_stopCFM		= (flags & B3_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
-			limot.m_stopERP		= (flags & B3_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
-			if(m_useOffsetForConstraintFrame)
+			limot.m_normalCFM = (flags & B3_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
+			limot.m_stopCFM = (flags & B3_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
+			limot.m_stopERP = (flags & B3_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
+			if (m_useOffsetForConstraintFrame)
 			{
 				int indx1 = (i + 1) % 3;
 				int indx2 = (i + 2) % 3;
-				int rotAllowed = 1; // rotations around orthos to current axis
-				if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
+				int rotAllowed = 1;  // rotations around orthos to current axis
+				if (m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
 				{
 					rotAllowed = 0;
 				}
-				row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
+				row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
 			}
 			else
 			{
-				row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
+				row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0);
 			}
 		}
 	}
 	return row;
 }
 
-
-
-int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2 *info, int row_offset, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB)
+int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2* info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB)
 {
-	b3Generic6DofConstraint * d6constraint = this;
+	b3Generic6DofConstraint* d6constraint = this;
 	int row = row_offset;
 	//solve angular limits
-	for (int i=0;i<3 ;i++ )
+	for (int i = 0; i < 3; i++)
 	{
-		if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
+		if (d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
 		{
 			b3Vector3 axis = d6constraint->getAxis(i);
 			int flags = m_flags >> ((i + 3) * B3_6DOF_FLAGS_AXIS_SHIFT);
-			if(!(flags & B3_6DOF_FLAGS_CFM_NORM))
+			if (!(flags & B3_6DOF_FLAGS_CFM_NORM))
 			{
 				m_angularLimits[i].m_normalCFM = info->cfm[0];
 			}
-			if(!(flags & B3_6DOF_FLAGS_CFM_STOP))
+			if (!(flags & B3_6DOF_FLAGS_CFM_STOP))
 			{
 				m_angularLimits[i].m_stopCFM = info->cfm[0];
 			}
-			if(!(flags & B3_6DOF_FLAGS_ERP_STOP))
+			if (!(flags & B3_6DOF_FLAGS_ERP_STOP))
 			{
 				m_angularLimits[i].m_stopERP = info->erp;
 			}
 			row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
-												transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
+										 transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
 		}
 	}
 
 	return row;
 }
 
-
-
-
-void	b3Generic6DofConstraint::updateRHS(b3Scalar	timeStep)
+void b3Generic6DofConstraint::updateRHS(b3Scalar timeStep)
 {
 	(void)timeStep;
-
 }
 
-
-void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB,const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies)
 {
 	m_frameInA = frameA;
 	m_frameInB = frameB;
@@ -455,33 +400,27 @@ void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Trans
 	calculateTransforms(bodies);
 }
 
-
-
 b3Vector3 b3Generic6DofConstraint::getAxis(int axis_index) const
 {
 	return m_calculatedAxis[axis_index];
 }
 
-
-b3Scalar	b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const
+b3Scalar b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const
 {
 	return m_calculatedLinearDiff[axisIndex];
 }
 
-
 b3Scalar b3Generic6DofConstraint::getAngle(int axisIndex) const
 {
 	return m_calculatedAxisAngleDiff[axisIndex];
 }
 
-
-
 void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies)
 {
 	b3Scalar imA = bodies[m_rbA].m_invMass;
 	b3Scalar imB = bodies[m_rbB].m_invMass;
 	b3Scalar weight;
-	if(imB == b3Scalar(0.0))
+	if (imB == b3Scalar(0.0))
 	{
 		weight = b3Scalar(1.0);
 	}
@@ -495,47 +434,43 @@ void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies)
 	return;
 }
 
-
-
 void b3Generic6DofConstraint::calculateLinearInfo()
 {
 	m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
 	m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
-	for(int i = 0; i < 3; i++)
+	for (int i = 0; i < 3; i++)
 	{
 		m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
 		m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
 	}
 }
 
-
-
 int b3Generic6DofConstraint::get_limit_motor_info2(
-	b3RotationalLimitMotor * limot,
-	const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,
-	b3ConstraintInfo2 *info, int row, b3Vector3& ax1, int rotational,int rotAllowed)
+	b3RotationalLimitMotor* limot,
+	const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB,
+	b3ConstraintInfo2* info, int row, b3Vector3& ax1, int rotational, int rotAllowed)
 {
-    int srow = row * info->rowskip;
-    bool powered = limot->m_enableMotor;
-    int limit = limot->m_currentLimit;
-    if (powered || limit)
-    {   // if the joint is powered, or has joint limits, add in the extra row
-        b3Scalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
-        b3Scalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
+	int srow = row * info->rowskip;
+	bool powered = limot->m_enableMotor;
+	int limit = limot->m_currentLimit;
+	if (powered || limit)
+	{  // if the joint is powered, or has joint limits, add in the extra row
+		b3Scalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
+		b3Scalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
 		if (J1)
 		{
-			J1[srow+0] = ax1[0];
-			J1[srow+1] = ax1[1];
-			J1[srow+2] = ax1[2];
+			J1[srow + 0] = ax1[0];
+			J1[srow + 1] = ax1[1];
+			J1[srow + 2] = ax1[2];
 		}
 		if (J2)
 		{
-			J2[srow+0] = -ax1[0];
-			J2[srow+1] = -ax1[1];
-			J2[srow+2] = -ax1[2];
+			J2[srow + 0] = -ax1[0];
+			J2[srow + 1] = -ax1[1];
+			J2[srow + 2] = -ax1[2];
 		}
-		if((!rotational))
-        {
+		if ((!rotational))
+		{
 			if (m_useOffsetForConstraintFrame)
 			{
 				b3Vector3 tmpA, tmpB, relA, relB;
@@ -558,55 +493,56 @@ int b3Generic6DofConstraint::get_limit_motor_info2(
 				relB = orthoB - totalDist * m_factB;
 				tmpA = relA.cross(ax1);
 				tmpB = relB.cross(ax1);
-				if(m_hasStaticBody && (!rotAllowed))
+				if (m_hasStaticBody && (!rotAllowed))
 				{
 					tmpA *= m_factA;
 					tmpB *= m_factB;
 				}
 				int i;
-				for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
-				for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
-			} else
+				for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
+				for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
+			}
+			else
 			{
-				b3Vector3 ltd;	// Linear Torque Decoupling vector
+				b3Vector3 ltd;  // Linear Torque Decoupling vector
 				b3Vector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
 				ltd = c.cross(ax1);
-				info->m_J1angularAxis[srow+0] = ltd[0];
-				info->m_J1angularAxis[srow+1] = ltd[1];
-				info->m_J1angularAxis[srow+2] = ltd[2];
+				info->m_J1angularAxis[srow + 0] = ltd[0];
+				info->m_J1angularAxis[srow + 1] = ltd[1];
+				info->m_J1angularAxis[srow + 2] = ltd[2];
 
 				c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
 				ltd = -c.cross(ax1);
-				info->m_J2angularAxis[srow+0] = ltd[0];
-				info->m_J2angularAxis[srow+1] = ltd[1];
-				info->m_J2angularAxis[srow+2] = ltd[2];
+				info->m_J2angularAxis[srow + 0] = ltd[0];
+				info->m_J2angularAxis[srow + 1] = ltd[1];
+				info->m_J2angularAxis[srow + 2] = ltd[2];
 			}
-        }
-        // if we're limited low and high simultaneously, the joint motor is
-        // ineffective
-        if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
-        info->m_constraintError[srow] = b3Scalar(0.f);
-        if (powered)
-        {
+		}
+		// if we're limited low and high simultaneously, the joint motor is
+		// ineffective
+		if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
+		info->m_constraintError[srow] = b3Scalar(0.f);
+		if (powered)
+		{
 			info->cfm[srow] = limot->m_normalCFM;
-            if(!limit)
-            {
+			if (!limit)
+			{
 				b3Scalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
 
-				b3Scalar mot_fact = getMotorFactor(	limot->m_currentPosition, 
-													limot->m_loLimit,
-													limot->m_hiLimit, 
-													tag_vel, 
-													info->fps * limot->m_stopERP);
+				b3Scalar mot_fact = getMotorFactor(limot->m_currentPosition,
+												   limot->m_loLimit,
+												   limot->m_hiLimit,
+												   tag_vel,
+												   info->fps * limot->m_stopERP);
 				info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
-                info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
-                info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
-            }
-        }
-        if(limit)
-        {
-            b3Scalar k = info->fps * limot->m_stopERP;
-			if(!rotational)
+				info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
+				info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
+			}
+		}
+		if (limit)
+		{
+			b3Scalar k = info->fps * limot->m_stopERP;
+			if (!rotational)
 			{
 				info->m_constraintError[srow] += k * limot->m_currentLimitError;
 			}
@@ -615,116 +551,112 @@ int b3Generic6DofConstraint::get_limit_motor_info2(
 				info->m_constraintError[srow] += -k * limot->m_currentLimitError;
 			}
 			info->cfm[srow] = limot->m_stopCFM;
-            if (limot->m_loLimit == limot->m_hiLimit)
-            {   // limited low and high simultaneously
-                info->m_lowerLimit[srow] = -B3_INFINITY;
-                info->m_upperLimit[srow] = B3_INFINITY;
-            }
-            else
-            {
-                if (limit == 1)
-                {
-                    info->m_lowerLimit[srow] = 0;
-                    info->m_upperLimit[srow] = B3_INFINITY;
-                }
-                else
-                {
-                    info->m_lowerLimit[srow] = -B3_INFINITY;
-                    info->m_upperLimit[srow] = 0;
-                }
-                // deal with bounce
-                if (limot->m_bounce > 0)
-                {
-                    // calculate joint velocity
-                    b3Scalar vel;
-                    if (rotational)
-                    {
-                        vel = angVelA.dot(ax1);
-//make sure that if no body -> angVelB == zero vec
-//                        if (body1)
-                            vel -= angVelB.dot(ax1);
-                    }
-                    else
-                    {
-                        vel = linVelA.dot(ax1);
-//make sure that if no body -> angVelB == zero vec
-//                        if (body1)
-                            vel -= linVelB.dot(ax1);
-                    }
-                    // only apply bounce if the velocity is incoming, and if the
-                    // resulting c[] exceeds what we already have.
-                    if (limit == 1)
-                    {
-                        if (vel < 0)
-                        {
-                            b3Scalar newc = -limot->m_bounce* vel;
-                            if (newc > info->m_constraintError[srow]) 
+			if (limot->m_loLimit == limot->m_hiLimit)
+			{  // limited low and high simultaneously
+				info->m_lowerLimit[srow] = -B3_INFINITY;
+				info->m_upperLimit[srow] = B3_INFINITY;
+			}
+			else
+			{
+				if (limit == 1)
+				{
+					info->m_lowerLimit[srow] = 0;
+					info->m_upperLimit[srow] = B3_INFINITY;
+				}
+				else
+				{
+					info->m_lowerLimit[srow] = -B3_INFINITY;
+					info->m_upperLimit[srow] = 0;
+				}
+				// deal with bounce
+				if (limot->m_bounce > 0)
+				{
+					// calculate joint velocity
+					b3Scalar vel;
+					if (rotational)
+					{
+						vel = angVelA.dot(ax1);
+						//make sure that if no body -> angVelB == zero vec
+						//                        if (body1)
+						vel -= angVelB.dot(ax1);
+					}
+					else
+					{
+						vel = linVelA.dot(ax1);
+						//make sure that if no body -> angVelB == zero vec
+						//                        if (body1)
+						vel -= linVelB.dot(ax1);
+					}
+					// only apply bounce if the velocity is incoming, and if the
+					// resulting c[] exceeds what we already have.
+					if (limit == 1)
+					{
+						if (vel < 0)
+						{
+							b3Scalar newc = -limot->m_bounce * vel;
+							if (newc > info->m_constraintError[srow])
 								info->m_constraintError[srow] = newc;
-                        }
-                    }
-                    else
-                    {
-                        if (vel > 0)
-                        {
-                            b3Scalar newc = -limot->m_bounce * vel;
-                            if (newc < info->m_constraintError[srow]) 
+						}
+					}
+					else
+					{
+						if (vel > 0)
+						{
+							b3Scalar newc = -limot->m_bounce * vel;
+							if (newc < info->m_constraintError[srow])
 								info->m_constraintError[srow] = newc;
-                        }
-                    }
-                }
-            }
-        }
-        return 1;
-    }
-    else return 0;
+						}
+					}
+				}
+			}
+		}
+		return 1;
+	}
+	else
+		return 0;
 }
 
-
-
-
-
-
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
-	///If no axis is provided, it uses the default axis for this constraint.
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+///If no axis is provided, it uses the default axis for this constraint.
 void b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis)
 {
-	if((axis >= 0) && (axis < 3))
+	if ((axis >= 0) && (axis < 3))
 	{
-		switch(num)
+		switch (num)
 		{
-			case B3_CONSTRAINT_STOP_ERP : 
+			case B3_CONSTRAINT_STOP_ERP:
 				m_linearLimits.m_stopERP[axis] = value;
 				m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			case B3_CONSTRAINT_STOP_CFM : 
+			case B3_CONSTRAINT_STOP_CFM:
 				m_linearLimits.m_stopCFM[axis] = value;
 				m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			case B3_CONSTRAINT_CFM : 
+			case B3_CONSTRAINT_CFM:
 				m_linearLimits.m_normalCFM[axis] = value;
 				m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			default : 
+			default:
 				b3AssertConstrParams(0);
 		}
 	}
-	else if((axis >=3) && (axis < 6))
+	else if ((axis >= 3) && (axis < 6))
 	{
-		switch(num)
+		switch (num)
 		{
-			case B3_CONSTRAINT_STOP_ERP : 
+			case B3_CONSTRAINT_STOP_ERP:
 				m_angularLimits[axis - 3].m_stopERP = value;
 				m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			case B3_CONSTRAINT_STOP_CFM : 
+			case B3_CONSTRAINT_STOP_CFM:
 				m_angularLimits[axis - 3].m_stopCFM = value;
 				m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			case B3_CONSTRAINT_CFM : 
+			case B3_CONSTRAINT_CFM:
 				m_angularLimits[axis - 3].m_normalCFM = value;
 				m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
 				break;
-			default : 
+			default:
 				b3AssertConstrParams(0);
 		}
 	}
@@ -734,47 +666,47 @@ void b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis)
 	}
 }
 
-	///return the local value of parameter
-b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const 
+///return the local value of parameter
+b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const
 {
 	b3Scalar retVal = 0;
-	if((axis >= 0) && (axis < 3))
+	if ((axis >= 0) && (axis < 3))
 	{
-		switch(num)
+		switch (num)
 		{
-			case B3_CONSTRAINT_STOP_ERP : 
+			case B3_CONSTRAINT_STOP_ERP:
 				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_linearLimits.m_stopERP[axis];
 				break;
-			case B3_CONSTRAINT_STOP_CFM : 
+			case B3_CONSTRAINT_STOP_CFM:
 				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_linearLimits.m_stopCFM[axis];
 				break;
-			case B3_CONSTRAINT_CFM : 
+			case B3_CONSTRAINT_CFM:
 				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_linearLimits.m_normalCFM[axis];
 				break;
-			default : 
+			default:
 				b3AssertConstrParams(0);
 		}
 	}
-	else if((axis >=3) && (axis < 6))
+	else if ((axis >= 3) && (axis < 6))
 	{
-		switch(num)
+		switch (num)
 		{
-			case B3_CONSTRAINT_STOP_ERP : 
+			case B3_CONSTRAINT_STOP_ERP:
 				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_angularLimits[axis - 3].m_stopERP;
 				break;
-			case B3_CONSTRAINT_STOP_CFM : 
+			case B3_CONSTRAINT_STOP_CFM:
 				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_angularLimits[axis - 3].m_stopCFM;
 				break;
-			case B3_CONSTRAINT_CFM : 
+			case B3_CONSTRAINT_CFM:
 				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
 				retVal = m_angularLimits[axis - 3].m_normalCFM;
 				break;
-			default : 
+			default:
 				b3AssertConstrParams(0);
 		}
 	}
@@ -785,23 +717,21 @@ b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const
 	return retVal;
 }
 
- 
-
-void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1,const b3Vector3& axis2, const b3RigidBodyData* bodies)
+void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies)
 {
 	b3Vector3 zAxis = axis1.normalized();
 	b3Vector3 yAxis = axis2.normalized();
-	b3Vector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
-	
+	b3Vector3 xAxis = yAxis.cross(zAxis);  // we want right coordinate system
+
 	b3Transform frameInW;
 	frameInW.setIdentity();
-	frameInW.getBasis().setValue(	xAxis[0], yAxis[0], zAxis[0],	
-	                                xAxis[1], yAxis[1], zAxis[1],
-	                               xAxis[2], yAxis[2], zAxis[2]);
-	
+	frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
+								 xAxis[1], yAxis[1], zAxis[1],
+								 xAxis[2], yAxis[2], zAxis[2]);
+
 	// now get constraint frame in local coordinate systems
 	m_frameInA = getCenterOfMassTransform(bodies[m_rbA]).inverse() * frameInW;
 	m_frameInB = getCenterOfMassTransform(bodies[m_rbB]).inverse() * frameInW;
-	
+
 	calculateTransforms(bodies);
 }
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h
index 169b1b94ad..1597809db3 100644
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h
@@ -23,7 +23,6 @@ email: projectileman@yahoo.com
 http://gimpact.sf.net
 */
 
-
 #ifndef B3_GENERIC_6DOF_CONSTRAINT_H
 #define B3_GENERIC_6DOF_CONSTRAINT_H
 
@@ -33,88 +32,83 @@ http://gimpact.sf.net
 
 struct b3RigidBodyData;
 
-
-
-
 //! Rotation Limit structure for generic joints
 class b3RotationalLimitMotor
 {
 public:
-    //! limit_parameters
-    //!@{
-    b3Scalar m_loLimit;//!< joint limit
-    b3Scalar m_hiLimit;//!< joint limit
-    b3Scalar m_targetVelocity;//!< target motor velocity
-    b3Scalar m_maxMotorForce;//!< max force on motor
-    b3Scalar m_maxLimitForce;//!< max force on limit
-    b3Scalar m_damping;//!< Damping.
-    b3Scalar m_limitSoftness;//! Relaxation factor
-    b3Scalar m_normalCFM;//!< Constraint force mixing factor
-    b3Scalar m_stopERP;//!< Error tolerance factor when joint is at limit
-    b3Scalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
-    b3Scalar m_bounce;//!< restitution factor
-    bool m_enableMotor;
-
-    //!@}
-
-    //! temp_variables
-    //!@{
-    b3Scalar m_currentLimitError;//!  How much is violated this limit
-    b3Scalar m_currentPosition;     //!  current value of angle 
-    int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
-    b3Scalar m_accumulatedImpulse;
-    //!@}
-
-    b3RotationalLimitMotor()
-    {
-    	m_accumulatedImpulse = 0.f;
-        m_targetVelocity = 0;
-        m_maxMotorForce = 6.0f;
-        m_maxLimitForce = 300.0f;
-        m_loLimit = 1.0f;
-        m_hiLimit = -1.0f;
+	//! limit_parameters
+	//!@{
+	b3Scalar m_loLimit;         //!< joint limit
+	b3Scalar m_hiLimit;         //!< joint limit
+	b3Scalar m_targetVelocity;  //!< target motor velocity
+	b3Scalar m_maxMotorForce;   //!< max force on motor
+	b3Scalar m_maxLimitForce;   //!< max force on limit
+	b3Scalar m_damping;         //!< Damping.
+	b3Scalar m_limitSoftness;   //! Relaxation factor
+	b3Scalar m_normalCFM;       //!< Constraint force mixing factor
+	b3Scalar m_stopERP;         //!< Error tolerance factor when joint is at limit
+	b3Scalar m_stopCFM;         //!< Constraint force mixing factor when joint is at limit
+	b3Scalar m_bounce;          //!< restitution factor
+	bool m_enableMotor;
+
+	//!@}
+
+	//! temp_variables
+	//!@{
+	b3Scalar m_currentLimitError;  //!  How much is violated this limit
+	b3Scalar m_currentPosition;    //!  current value of angle
+	int m_currentLimit;            //!< 0=free, 1=at lo limit, 2=at hi limit
+	b3Scalar m_accumulatedImpulse;
+	//!@}
+
+	b3RotationalLimitMotor()
+	{
+		m_accumulatedImpulse = 0.f;
+		m_targetVelocity = 0;
+		m_maxMotorForce = 6.0f;
+		m_maxLimitForce = 300.0f;
+		m_loLimit = 1.0f;
+		m_hiLimit = -1.0f;
 		m_normalCFM = 0.f;
 		m_stopERP = 0.2f;
 		m_stopCFM = 0.f;
-        m_bounce = 0.0f;
-        m_damping = 1.0f;
-        m_limitSoftness = 0.5f;
-        m_currentLimit = 0;
-        m_currentLimitError = 0;
-        m_enableMotor = false;
-    }
-
-    b3RotationalLimitMotor(const b3RotationalLimitMotor & limot)
-    {
-        m_targetVelocity = limot.m_targetVelocity;
-        m_maxMotorForce = limot.m_maxMotorForce;
-        m_limitSoftness = limot.m_limitSoftness;
-        m_loLimit = limot.m_loLimit;
-        m_hiLimit = limot.m_hiLimit;
+		m_bounce = 0.0f;
+		m_damping = 1.0f;
+		m_limitSoftness = 0.5f;
+		m_currentLimit = 0;
+		m_currentLimitError = 0;
+		m_enableMotor = false;
+	}
+
+	b3RotationalLimitMotor(const b3RotationalLimitMotor& limot)
+	{
+		m_targetVelocity = limot.m_targetVelocity;
+		m_maxMotorForce = limot.m_maxMotorForce;
+		m_limitSoftness = limot.m_limitSoftness;
+		m_loLimit = limot.m_loLimit;
+		m_hiLimit = limot.m_hiLimit;
 		m_normalCFM = limot.m_normalCFM;
 		m_stopERP = limot.m_stopERP;
-		m_stopCFM =	limot.m_stopCFM;
-        m_bounce = limot.m_bounce;
-        m_currentLimit = limot.m_currentLimit;
-        m_currentLimitError = limot.m_currentLimitError;
-        m_enableMotor = limot.m_enableMotor;
-    }
-
-
+		m_stopCFM = limot.m_stopCFM;
+		m_bounce = limot.m_bounce;
+		m_currentLimit = limot.m_currentLimit;
+		m_currentLimitError = limot.m_currentLimitError;
+		m_enableMotor = limot.m_enableMotor;
+	}
 
 	//! Is limited
-    bool isLimited()
-    {
-    	if(m_loLimit > m_hiLimit) return false;
-    	return true;
-    }
+	bool isLimited()
+	{
+		if (m_loLimit > m_hiLimit) return false;
+		return true;
+	}
 
 	//! Need apply correction
-    bool needApplyTorques()
-    {
-    	if(m_currentLimit == 0 && m_enableMotor == false) return false;
-    	return true;
-    }
+	bool needApplyTorques()
+	{
+		if (m_currentLimit == 0 && m_enableMotor == false) return false;
+		return true;
+	}
 
 	//! calculates  error
 	/*!
@@ -123,104 +117,98 @@ public:
 	int testLimitValue(b3Scalar test_value);
 
 	//! apply the correction impulses for two bodies
-    b3Scalar solveAngularLimits(b3Scalar timeStep,b3Vector3& axis, b3Scalar jacDiagABInv,b3RigidBodyData * body0, b3RigidBodyData * body1);
-
+	b3Scalar solveAngularLimits(b3Scalar timeStep, b3Vector3& axis, b3Scalar jacDiagABInv, b3RigidBodyData* body0, b3RigidBodyData* body1);
 };
 
-
-
 class b3TranslationalLimitMotor
 {
 public:
-	b3Vector3 m_lowerLimit;//!< the constraint lower limits
-    b3Vector3 m_upperLimit;//!< the constraint upper limits
-    b3Vector3 m_accumulatedImpulse;
-    //! Linear_Limit_parameters
-    //!@{
-	b3Vector3	m_normalCFM;//!< Constraint force mixing factor
-    b3Vector3	m_stopERP;//!< Error tolerance factor when joint is at limit
-	b3Vector3	m_stopCFM;//!< Constraint force mixing factor when joint is at limit
-    b3Vector3	m_targetVelocity;//!< target motor velocity
-    b3Vector3	m_maxMotorForce;//!< max force on motor
-    b3Vector3	m_currentLimitError;//!  How much is violated this limit
-    b3Vector3	m_currentLinearDiff;//!  Current relative offset of constraint frames
-	b3Scalar	m_limitSoftness;//!< Softness for linear limit
-    b3Scalar	m_damping;//!< Damping for linear limit
-    b3Scalar	m_restitution;//! Bounce parameter for linear limit
+	b3Vector3 m_lowerLimit;  //!< the constraint lower limits
+	b3Vector3 m_upperLimit;  //!< the constraint upper limits
+	b3Vector3 m_accumulatedImpulse;
+	//! Linear_Limit_parameters
+	//!@{
+	b3Vector3 m_normalCFM;          //!< Constraint force mixing factor
+	b3Vector3 m_stopERP;            //!< Error tolerance factor when joint is at limit
+	b3Vector3 m_stopCFM;            //!< Constraint force mixing factor when joint is at limit
+	b3Vector3 m_targetVelocity;     //!< target motor velocity
+	b3Vector3 m_maxMotorForce;      //!< max force on motor
+	b3Vector3 m_currentLimitError;  //!  How much is violated this limit
+	b3Vector3 m_currentLinearDiff;  //!  Current relative offset of constraint frames
+	b3Scalar m_limitSoftness;       //!< Softness for linear limit
+	b3Scalar m_damping;             //!< Damping for linear limit
+	b3Scalar m_restitution;         //! Bounce parameter for linear limit
 	//!@}
-	bool		m_enableMotor[3];
-	int			m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
-
-    b3TranslationalLimitMotor()
-    {
-    	m_lowerLimit.setValue(0.f,0.f,0.f);
-    	m_upperLimit.setValue(0.f,0.f,0.f);
-    	m_accumulatedImpulse.setValue(0.f,0.f,0.f);
+	bool m_enableMotor[3];
+	int m_currentLimit[3];  //!< 0=free, 1=at lower limit, 2=at upper limit
+
+	b3TranslationalLimitMotor()
+	{
+		m_lowerLimit.setValue(0.f, 0.f, 0.f);
+		m_upperLimit.setValue(0.f, 0.f, 0.f);
+		m_accumulatedImpulse.setValue(0.f, 0.f, 0.f);
 		m_normalCFM.setValue(0.f, 0.f, 0.f);
 		m_stopERP.setValue(0.2f, 0.2f, 0.2f);
 		m_stopCFM.setValue(0.f, 0.f, 0.f);
 
-    	m_limitSoftness = 0.7f;
-    	m_damping = b3Scalar(1.0f);
-    	m_restitution = b3Scalar(0.5f);
-		for(int i=0; i < 3; i++) 
+		m_limitSoftness = 0.7f;
+		m_damping = b3Scalar(1.0f);
+		m_restitution = b3Scalar(0.5f);
+		for (int i = 0; i < 3; i++)
 		{
 			m_enableMotor[i] = false;
 			m_targetVelocity[i] = b3Scalar(0.f);
 			m_maxMotorForce[i] = b3Scalar(0.f);
 		}
-    }
+	}
 
-    b3TranslationalLimitMotor(const b3TranslationalLimitMotor & other )
-    {
-    	m_lowerLimit = other.m_lowerLimit;
-    	m_upperLimit = other.m_upperLimit;
-    	m_accumulatedImpulse = other.m_accumulatedImpulse;
+	b3TranslationalLimitMotor(const b3TranslationalLimitMotor& other)
+	{
+		m_lowerLimit = other.m_lowerLimit;
+		m_upperLimit = other.m_upperLimit;
+		m_accumulatedImpulse = other.m_accumulatedImpulse;
 
-    	m_limitSoftness = other.m_limitSoftness ;
-    	m_damping = other.m_damping;
-    	m_restitution = other.m_restitution;
+		m_limitSoftness = other.m_limitSoftness;
+		m_damping = other.m_damping;
+		m_restitution = other.m_restitution;
 		m_normalCFM = other.m_normalCFM;
 		m_stopERP = other.m_stopERP;
 		m_stopCFM = other.m_stopCFM;
 
-		for(int i=0; i < 3; i++) 
+		for (int i = 0; i < 3; i++)
 		{
 			m_enableMotor[i] = other.m_enableMotor[i];
 			m_targetVelocity[i] = other.m_targetVelocity[i];
 			m_maxMotorForce[i] = other.m_maxMotorForce[i];
 		}
-    }
+	}
 
-    //! Test limit
+	//! Test limit
 	/*!
     - free means upper < lower,
     - locked means upper == lower
     - limited means upper > lower
     - limitIndex: first 3 are linear, next 3 are angular
     */
-    inline bool	isLimited(int limitIndex)
-    {
-       return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
-    }
-    inline bool needApplyForce(int limitIndex)
-    {
-    	if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
-    	return true;
-    }
+	inline bool isLimited(int limitIndex)
+	{
+		return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
+	}
+	inline bool needApplyForce(int limitIndex)
+	{
+		if (m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
+		return true;
+	}
 	int testLimitValue(int limitIndex, b3Scalar test_value);
 
-
-    b3Scalar solveLinearAxis(
-    	b3Scalar timeStep,
-        b3Scalar jacDiagABInv,
-        b3RigidBodyData& body1,const b3Vector3 &pointInA,
-        b3RigidBodyData& body2,const b3Vector3 &pointInB,
-        int limit_index,
-        const b3Vector3 & axis_normal_on_a,
-		const b3Vector3 & anchorPos);
-
-
+	b3Scalar solveLinearAxis(
+		b3Scalar timeStep,
+		b3Scalar jacDiagABInv,
+		b3RigidBodyData& body1, const b3Vector3& pointInA,
+		b3RigidBodyData& body2, const b3Vector3& pointInB,
+		int limit_index,
+		const b3Vector3& axis_normal_on_a,
+		const b3Vector3& anchorPos);
 };
 
 enum b36DofFlags
@@ -229,8 +217,7 @@ enum b36DofFlags
 	B3_6DOF_FLAGS_CFM_STOP = 2,
 	B3_6DOF_FLAGS_ERP_STOP = 4
 };
-#define B3_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
-
+#define B3_6DOF_FLAGS_AXIS_SHIFT 3  // bits per axis
 
 /// b3Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
 /*!
@@ -268,240 +255,229 @@ This brings support for limit parameters and motors. </li>
 </ul>
 
 */
-B3_ATTRIBUTE_ALIGNED16(class) b3Generic6DofConstraint : public b3TypedConstraint
+B3_ATTRIBUTE_ALIGNED16(class)
+b3Generic6DofConstraint : public b3TypedConstraint
 {
 protected:
-
 	//! relative_frames
-    //!@{
-	b3Transform	m_frameInA;//!< the constraint space w.r.t body A
-    b3Transform	m_frameInB;//!< the constraint space w.r.t body B
-    //!@}
+	//!@{
+	b3Transform m_frameInA;  //!< the constraint space w.r.t body A
+	b3Transform m_frameInB;  //!< the constraint space w.r.t body B
+	//!@}
 
-    //! Jacobians
-    //!@{
-//    b3JacobianEntry	m_jacLinear[3];//!< 3 orthogonal linear constraints
-//    b3JacobianEntry	m_jacAng[3];//!< 3 orthogonal angular constraints
-    //!@}
+	//! Jacobians
+	//!@{
+	//    b3JacobianEntry	m_jacLinear[3];//!< 3 orthogonal linear constraints
+	//    b3JacobianEntry	m_jacAng[3];//!< 3 orthogonal angular constraints
+	//!@}
 
 	//! Linear_Limit_parameters
-    //!@{
-    b3TranslationalLimitMotor m_linearLimits;
-    //!@}
-
-
-    //! hinge_parameters
-    //!@{
-    b3RotationalLimitMotor m_angularLimits[3];
+	//!@{
+	b3TranslationalLimitMotor m_linearLimits;
 	//!@}
 
+	//! hinge_parameters
+	//!@{
+	b3RotationalLimitMotor m_angularLimits[3];
+	//!@}
 
 protected:
-    //! temporal variables
-    //!@{
-    b3Transform m_calculatedTransformA;
-    b3Transform m_calculatedTransformB;
-    b3Vector3 m_calculatedAxisAngleDiff;
-    b3Vector3 m_calculatedAxis[3];
-    b3Vector3 m_calculatedLinearDiff;
-    b3Scalar m_timeStep;
-	b3Scalar	m_factA;
-	b3Scalar	m_factB;
-	bool		m_hasStaticBody;
-    
-	b3Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
+	//! temporal variables
+	//!@{
+	b3Transform m_calculatedTransformA;
+	b3Transform m_calculatedTransformB;
+	b3Vector3 m_calculatedAxisAngleDiff;
+	b3Vector3 m_calculatedAxis[3];
+	b3Vector3 m_calculatedLinearDiff;
+	b3Scalar m_timeStep;
+	b3Scalar m_factA;
+	b3Scalar m_factB;
+	bool m_hasStaticBody;
 
-    bool	m_useLinearReferenceFrameA;
-	bool	m_useOffsetForConstraintFrame;
-    
-	int		m_flags;
+	b3Vector3 m_AnchorPos;  // point betwen pivots of bodies A and B to solve linear axes
 
-    //!@}
+	bool m_useLinearReferenceFrameA;
+	bool m_useOffsetForConstraintFrame;
 
-    b3Generic6DofConstraint&	operator=(b3Generic6DofConstraint&	other)
-    {
-        b3Assert(0);
-        (void) other;
-        return *this;
-    }
+	int m_flags;
 
+	//!@}
 
-	int setAngularLimits(b3ConstraintInfo2 *info, int row_offset,const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB);
+	b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other)
+	{
+		b3Assert(0);
+		(void)other;
+		return *this;
+	}
 
-	int setLinearLimits(b3ConstraintInfo2 *info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB);
+	int setAngularLimits(b3ConstraintInfo2 * info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB);
 
+	int setLinearLimits(b3ConstraintInfo2 * info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB);
 
 	// tests linear limits
 	void calculateLinearInfo();
 
 	//! calcs the euler angles between the two bodies.
-    void calculateAngleInfo();
-
-
+	void calculateAngleInfo();
 
 public:
-
 	B3_DECLARE_ALIGNED_ALLOCATOR();
-	
-    b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB ,bool useLinearReferenceFrameA,const b3RigidBodyData* bodies);
-    
+
+	b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies);
+
 	//! Calcs global transform of the offsets
 	/*!
 	Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
 	\sa b3Generic6DofConstraint.getCalculatedTransformA , b3Generic6DofConstraint.getCalculatedTransformB, b3Generic6DofConstraint.calculateAngleInfo
 	*/
-    void calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyData* bodies);
+	void calculateTransforms(const b3Transform& transA, const b3Transform& transB, const b3RigidBodyData* bodies);
 
 	void calculateTransforms(const b3RigidBodyData* bodies);
 
 	//! Gets the global transform of the offset for body A
-    /*!
+	/*!
     \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
     */
-    const b3Transform & getCalculatedTransformA() const
-    {
-    	return m_calculatedTransformA;
-    }
+	const b3Transform& getCalculatedTransformA() const
+	{
+		return m_calculatedTransformA;
+	}
 
-    //! Gets the global transform of the offset for body B
-    /*!
+	//! Gets the global transform of the offset for body B
+	/*!
     \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
     */
-    const b3Transform & getCalculatedTransformB() const
-    {
-    	return m_calculatedTransformB;
-    }
-
-    const b3Transform & getFrameOffsetA() const
-    {
-    	return m_frameInA;
-    }
-
-    const b3Transform & getFrameOffsetB() const
-    {
-    	return m_frameInB;
-    }
-
-
-    b3Transform & getFrameOffsetA()
-    {
-    	return m_frameInA;
-    }
+	const b3Transform& getCalculatedTransformB() const
+	{
+		return m_calculatedTransformB;
+	}
 
-    b3Transform & getFrameOffsetB()
-    {
-    	return m_frameInB;
-    }
+	const b3Transform& getFrameOffsetA() const
+	{
+		return m_frameInA;
+	}
 
+	const b3Transform& getFrameOffsetB() const
+	{
+		return m_frameInB;
+	}
 
+	b3Transform& getFrameOffsetA()
+	{
+		return m_frameInA;
+	}
 
-	virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
+	b3Transform& getFrameOffsetB()
+	{
+		return m_frameInB;
+	}
 
-	void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
+	virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
 
-	virtual void getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyData* bodies);
+	void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
 
-	void getInfo2NonVirtual (b3ConstraintInfo2* info,const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyData* bodies);
+	virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
 
+	void getInfo2NonVirtual(b3ConstraintInfo2 * info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies);
 
-    void	updateRHS(b3Scalar	timeStep);
+	void updateRHS(b3Scalar timeStep);
 
 	//! Get the rotation axis in global coordinates
-    b3Vector3 getAxis(int axis_index) const;
+	b3Vector3 getAxis(int axis_index) const;
 
-    //! Get the relative Euler angle
-    /*!
+	//! Get the relative Euler angle
+	/*!
 	\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
 	*/
-    b3Scalar getAngle(int axis_index) const;
+	b3Scalar getAngle(int axis_index) const;
 
 	//! Get the relative position of the constraint pivot
-    /*!
+	/*!
 	\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
 	*/
 	b3Scalar getRelativePivotPosition(int axis_index) const;
 
-	void setFrames(const b3Transform & frameA, const b3Transform & frameB, const b3RigidBodyData* bodies);
+	void setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies);
 
 	//! Test angular limit.
 	/*!
 	Calculates angular correction and returns true if limit needs to be corrected.
 	\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
 	*/
-    bool testAngularLimitMotor(int axis_index);
+	bool testAngularLimitMotor(int axis_index);
 
-    void	setLinearLowerLimit(const b3Vector3& linearLower)
-    {
-    	m_linearLimits.m_lowerLimit = linearLower;
-    }
+	void setLinearLowerLimit(const b3Vector3& linearLower)
+	{
+		m_linearLimits.m_lowerLimit = linearLower;
+	}
 
-	void	getLinearLowerLimit(b3Vector3& linearLower)
+	void getLinearLowerLimit(b3Vector3 & linearLower)
 	{
 		linearLower = m_linearLimits.m_lowerLimit;
 	}
 
-	void	setLinearUpperLimit(const b3Vector3& linearUpper)
+	void setLinearUpperLimit(const b3Vector3& linearUpper)
 	{
 		m_linearLimits.m_upperLimit = linearUpper;
 	}
 
-	void	getLinearUpperLimit(b3Vector3& linearUpper)
+	void getLinearUpperLimit(b3Vector3 & linearUpper)
 	{
 		linearUpper = m_linearLimits.m_upperLimit;
 	}
 
-    void	setAngularLowerLimit(const b3Vector3& angularLower)
-    {
-		for(int i = 0; i < 3; i++) 
+	void setAngularLowerLimit(const b3Vector3& angularLower)
+	{
+		for (int i = 0; i < 3; i++)
 			m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]);
-    }
+	}
 
-	void	getAngularLowerLimit(b3Vector3& angularLower)
+	void getAngularLowerLimit(b3Vector3 & angularLower)
 	{
-		for(int i = 0; i < 3; i++) 
+		for (int i = 0; i < 3; i++)
 			angularLower[i] = m_angularLimits[i].m_loLimit;
 	}
 
-    void	setAngularUpperLimit(const b3Vector3& angularUpper)
-    {
-		for(int i = 0; i < 3; i++)
+	void setAngularUpperLimit(const b3Vector3& angularUpper)
+	{
+		for (int i = 0; i < 3; i++)
 			m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]);
-    }
+	}
 
-	void	getAngularUpperLimit(b3Vector3& angularUpper)
+	void getAngularUpperLimit(b3Vector3 & angularUpper)
 	{
-		for(int i = 0; i < 3; i++)
+		for (int i = 0; i < 3; i++)
 			angularUpper[i] = m_angularLimits[i].m_hiLimit;
 	}
 
 	//! Retrieves the angular limit informacion
-    b3RotationalLimitMotor * getRotationalLimitMotor(int index)
-    {
-    	return &m_angularLimits[index];
-    }
-
-    //! Retrieves the  limit informacion
-    b3TranslationalLimitMotor * getTranslationalLimitMotor()
-    {
-    	return &m_linearLimits;
-    }
-
-    //first 3 are linear, next 3 are angular
-    void setLimit(int axis, b3Scalar lo, b3Scalar hi)
-    {
-    	if(axis<3)
-    	{
-    		m_linearLimits.m_lowerLimit[axis] = lo;
-    		m_linearLimits.m_upperLimit[axis] = hi;
-    	}
-    	else
-    	{
+	b3RotationalLimitMotor* getRotationalLimitMotor(int index)
+	{
+		return &m_angularLimits[index];
+	}
+
+	//! Retrieves the  limit informacion
+	b3TranslationalLimitMotor* getTranslationalLimitMotor()
+	{
+		return &m_linearLimits;
+	}
+
+	//first 3 are linear, next 3 are angular
+	void setLimit(int axis, b3Scalar lo, b3Scalar hi)
+	{
+		if (axis < 3)
+		{
+			m_linearLimits.m_lowerLimit[axis] = lo;
+			m_linearLimits.m_upperLimit[axis] = hi;
+		}
+		else
+		{
 			lo = b3NormalizeAngle(lo);
 			hi = b3NormalizeAngle(hi);
-    		m_angularLimits[axis-3].m_loLimit = lo;
-    		m_angularLimits[axis-3].m_hiLimit = hi;
-    	}
-    }
+			m_angularLimits[axis - 3].m_loLimit = lo;
+			m_angularLimits[axis - 3].m_hiLimit = hi;
+		}
+	}
 
 	//! Test limit
 	/*!
@@ -510,41 +486,32 @@ public:
     - limited means upper > lower
     - limitIndex: first 3 are linear, next 3 are angular
     */
-    bool	isLimited(int limitIndex)
-    {
-    	if(limitIndex<3)
-    	{
+	bool isLimited(int limitIndex)
+	{
+		if (limitIndex < 3)
+		{
 			return m_linearLimits.isLimited(limitIndex);
+		}
+		return m_angularLimits[limitIndex - 3].isLimited();
+	}
 
-    	}
-        return m_angularLimits[limitIndex-3].isLimited();
-    }
-
-	virtual void calcAnchorPos(const b3RigidBodyData* bodies); // overridable
+	virtual void calcAnchorPos(const b3RigidBodyData* bodies);  // overridable
 
-	int get_limit_motor_info2(	b3RotationalLimitMotor * limot,
-								const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,
-								b3ConstraintInfo2 *info, int row, b3Vector3& ax1, int rotational, int rotAllowed = false);
+	int get_limit_motor_info2(b3RotationalLimitMotor * limot,
+							  const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB,
+							  b3ConstraintInfo2* info, int row, b3Vector3& ax1, int rotational, int rotAllowed = false);
 
 	// access for UseFrameOffset
 	bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
 	void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 	///If no axis is provided, it uses the default axis for this constraint.
-	virtual	void setParam(int num, b3Scalar value, int axis = -1);
+	virtual void setParam(int num, b3Scalar value, int axis = -1);
 	///return the local value of parameter
-	virtual	b3Scalar getParam(int num, int axis = -1) const;
-
-	void setAxis( const b3Vector3& axis1, const b3Vector3& axis2,const b3RigidBodyData* bodies);
+	virtual b3Scalar getParam(int num, int axis = -1) const;
 
-
-
-	
+	void setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies);
 };
 
-
-
-
-
-#endif //B3_GENERIC_6DOF_CONSTRAINT_H
+#endif  //B3_GENERIC_6DOF_CONSTRAINT_H
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h
index a55168eb38..13269debf6 100644
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h
@@ -18,7 +18,6 @@ subject to the following restrictions:
 
 #include "Bullet3Common/b3Matrix3x3.h"
 
-
 //notes:
 // Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
 // which makes the b3JacobianEntry memory layout 16 bytes
@@ -27,25 +26,26 @@ subject to the following restrictions:
 /// Jacobian entry is an abstraction that allows to describe constraints
 /// it can be used in combination with a constraint solver
 /// Can be used to relate the effect of an impulse to the constraint error
-B3_ATTRIBUTE_ALIGNED16(class) b3JacobianEntry
+B3_ATTRIBUTE_ALIGNED16(class)
+b3JacobianEntry
 {
 public:
-	b3JacobianEntry() {};
+	b3JacobianEntry(){};
 	//constraint between two different rigidbodies
 	b3JacobianEntry(
 		const b3Matrix3x3& world2A,
 		const b3Matrix3x3& world2B,
-		const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
+		const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
 		const b3Vector3& jointAxis,
-		const b3Vector3& inertiaInvA, 
+		const b3Vector3& inertiaInvA,
 		const b3Scalar massInvA,
 		const b3Vector3& inertiaInvB,
 		const b3Scalar massInvB)
-		:m_linearJointAxis(jointAxis)
+		: m_linearJointAxis(jointAxis)
 	{
-		m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
-		m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
-		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_aJ = world2A * (rel_pos1.cross(m_linearJointAxis));
+		m_bJ = world2B * (rel_pos2.cross(-m_linearJointAxis));
+		m_0MinvJt = inertiaInvA * m_aJ;
 		m_1MinvJt = inertiaInvB * m_bJ;
 		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
 
@@ -54,33 +54,31 @@ public:
 
 	//angular constraint between two different rigidbodies
 	b3JacobianEntry(const b3Vector3& jointAxis,
-		const b3Matrix3x3& world2A,
-		const b3Matrix3x3& world2B,
-		const b3Vector3& inertiaInvA,
-		const b3Vector3& inertiaInvB)
-		:m_linearJointAxis(b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)))
+					const b3Matrix3x3& world2A,
+					const b3Matrix3x3& world2B,
+					const b3Vector3& inertiaInvA,
+					const b3Vector3& inertiaInvB)
+		: m_linearJointAxis(b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.)))
 	{
-		m_aJ= world2A*jointAxis;
-		m_bJ = world2B*-jointAxis;
-		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_aJ = world2A * jointAxis;
+		m_bJ = world2B * -jointAxis;
+		m_0MinvJt = inertiaInvA * m_aJ;
 		m_1MinvJt = inertiaInvB * m_bJ;
-		m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+		m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
 
 		b3Assert(m_Adiag > b3Scalar(0.0));
 	}
 
 	//angular constraint between two different rigidbodies
 	b3JacobianEntry(const b3Vector3& axisInA,
-		const b3Vector3& axisInB,
-		const b3Vector3& inertiaInvA,
-		const b3Vector3& inertiaInvB)
-		: m_linearJointAxis(b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)))
-		, m_aJ(axisInA)
-		, m_bJ(-axisInB)
+					const b3Vector3& axisInB,
+					const b3Vector3& inertiaInvA,
+					const b3Vector3& inertiaInvB)
+		: m_linearJointAxis(b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.))), m_aJ(axisInA), m_bJ(-axisInB)
 	{
-		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_0MinvJt = inertiaInvA * m_aJ;
 		m_1MinvJt = inertiaInvB * m_bJ;
-		m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+		m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
 
 		b3Assert(m_Adiag > b3Scalar(0.0));
 	}
@@ -88,25 +86,25 @@ public:
 	//constraint on one rigidbody
 	b3JacobianEntry(
 		const b3Matrix3x3& world2A,
-		const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
+		const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
 		const b3Vector3& jointAxis,
-		const b3Vector3& inertiaInvA, 
+		const b3Vector3& inertiaInvA,
 		const b3Scalar massInvA)
-		:m_linearJointAxis(jointAxis)
+		: m_linearJointAxis(jointAxis)
 	{
-		m_aJ= world2A*(rel_pos1.cross(jointAxis));
-		m_bJ = world2A*(rel_pos2.cross(-jointAxis));
-		m_0MinvJt	= inertiaInvA * m_aJ;
-		m_1MinvJt = b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
+		m_aJ = world2A * (rel_pos1.cross(jointAxis));
+		m_bJ = world2A * (rel_pos2.cross(-jointAxis));
+		m_0MinvJt = inertiaInvA * m_aJ;
+		m_1MinvJt = b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
 		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
 
 		b3Assert(m_Adiag > b3Scalar(0.0));
 	}
 
-	b3Scalar	getDiagonal() const { return m_Adiag; }
+	b3Scalar getDiagonal() const { return m_Adiag; }
 
 	// for two constraints on the same rigidbody (for example vehicle friction)
-	b3Scalar	getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA) const
+	b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA) const
 	{
 		const b3JacobianEntry& jacA = *this;
 		b3Scalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
@@ -114,42 +112,39 @@ public:
 		return lin + ang;
 	}
 
-	
-
 	// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
-	b3Scalar	getNonDiagonal(const b3JacobianEntry& jacB,const b3Scalar massInvA,const b3Scalar massInvB) const
+	b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA, const b3Scalar massInvB) const
 	{
 		const b3JacobianEntry& jacA = *this;
 		b3Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
 		b3Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
 		b3Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
-		b3Vector3 lin0 = massInvA * lin ;
+		b3Vector3 lin0 = massInvA * lin;
 		b3Vector3 lin1 = massInvB * lin;
-		b3Vector3 sum = ang0+ang1+lin0+lin1;
-		return sum[0]+sum[1]+sum[2];
+		b3Vector3 sum = ang0 + ang1 + lin0 + lin1;
+		return sum[0] + sum[1] + sum[2];
 	}
 
-	b3Scalar getRelativeVelocity(const b3Vector3& linvelA,const b3Vector3& angvelA,const b3Vector3& linvelB,const b3Vector3& angvelB)
+	b3Scalar getRelativeVelocity(const b3Vector3& linvelA, const b3Vector3& angvelA, const b3Vector3& linvelB, const b3Vector3& angvelB)
 	{
 		b3Vector3 linrel = linvelA - linvelB;
-		b3Vector3 angvela  = angvelA * m_aJ;
-		b3Vector3 angvelb  = angvelB * m_bJ;
+		b3Vector3 angvela = angvelA * m_aJ;
+		b3Vector3 angvelb = angvelB * m_bJ;
 		linrel *= m_linearJointAxis;
 		angvela += angvelb;
 		angvela += linrel;
-		b3Scalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
+		b3Scalar rel_vel2 = angvela[0] + angvela[1] + angvela[2];
 		return rel_vel2 + B3_EPSILON;
 	}
-//private:
+	//private:
 
-	b3Vector3	m_linearJointAxis;
-	b3Vector3	m_aJ;
-	b3Vector3	m_bJ;
-	b3Vector3	m_0MinvJt;
-	b3Vector3	m_1MinvJt;
+	b3Vector3 m_linearJointAxis;
+	b3Vector3 m_aJ;
+	b3Vector3 m_bJ;
+	b3Vector3 m_0MinvJt;
+	b3Vector3 m_1MinvJt;
 	//Optimization: can be stored in the w/last component of one of the vectors
-	b3Scalar	m_Adiag;
-
+	b3Scalar m_Adiag;
 };
 
-#endif //B3_JACOBIAN_ENTRY_H
+#endif  //B3_JACOBIAN_ENTRY_H
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp
index de729d4556..b7050b1070 100644
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp
@@ -29,14 +29,13 @@ subject to the following restrictions:
 //#include "b3SolverBody.h"
 //#include "b3SolverConstraint.h"
 #include "Bullet3Common/b3AlignedObjectArray.h"
-#include <string.h> //for memset
+#include <string.h>  //for memset
 //#include "../../dynamics/basic_demo/Stubs/AdlContact4.h"
 #include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h"
 
-
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
 
-static b3Transform	getWorldTransform(b3RigidBodyData* rb)
+static b3Transform getWorldTransform(b3RigidBodyData* rb)
 {
 	b3Transform newTrans;
 	newTrans.setOrigin(rb->m_pos);
@@ -44,19 +43,17 @@ static b3Transform	getWorldTransform(b3RigidBodyData* rb)
 	return newTrans;
 }
 
-static const b3Matrix3x3&	getInvInertiaTensorWorld(b3InertiaData* inertia)
+static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaData* inertia)
 {
 	return inertia->m_invInertiaWorld;
 }
 
-
-
-static const b3Vector3&	getLinearVelocity(b3RigidBodyData* rb)
+static const b3Vector3& getLinearVelocity(b3RigidBodyData* rb)
 {
 	return rb->m_linVel;
 }
 
-static const b3Vector3&	getAngularVelocity(b3RigidBodyData* rb)
+static const b3Vector3& getAngularVelocity(b3RigidBodyData* rb)
 {
 	return rb->m_angVel;
 }
@@ -65,47 +62,46 @@ static b3Vector3 getVelocityInLocalPoint(b3RigidBodyData* rb, const b3Vector3& r
 {
 	//we also calculate lin/ang velocity for kinematic objects
 	return getLinearVelocity(rb) + getAngularVelocity(rb).cross(rel_pos);
-	
 }
 
-struct	b3ContactPoint
+struct b3ContactPoint
 {
-	b3Vector3	m_positionWorldOnA;
-	b3Vector3	m_positionWorldOnB;
-	b3Vector3	m_normalWorldOnB;
-	b3Scalar	m_appliedImpulse;
-	b3Scalar	m_distance;
-	b3Scalar	m_combinedRestitution;
+	b3Vector3 m_positionWorldOnA;
+	b3Vector3 m_positionWorldOnB;
+	b3Vector3 m_normalWorldOnB;
+	b3Scalar m_appliedImpulse;
+	b3Scalar m_distance;
+	b3Scalar m_combinedRestitution;
 
 	///information related to friction
-	b3Scalar	m_combinedFriction;
-	b3Vector3	m_lateralFrictionDir1;
-	b3Vector3	m_lateralFrictionDir2;
-	b3Scalar	m_appliedImpulseLateral1;
-	b3Scalar	m_appliedImpulseLateral2;	
-	b3Scalar	m_combinedRollingFriction;
-	b3Scalar	m_contactMotion1;
-	b3Scalar	m_contactMotion2;
-	b3Scalar	m_contactCFM1;
-	b3Scalar	m_contactCFM2;
-	
-	bool		m_lateralFrictionInitialized;
-
-	b3Vector3	getPositionWorldOnA()
+	b3Scalar m_combinedFriction;
+	b3Vector3 m_lateralFrictionDir1;
+	b3Vector3 m_lateralFrictionDir2;
+	b3Scalar m_appliedImpulseLateral1;
+	b3Scalar m_appliedImpulseLateral2;
+	b3Scalar m_combinedRollingFriction;
+	b3Scalar m_contactMotion1;
+	b3Scalar m_contactMotion2;
+	b3Scalar m_contactCFM1;
+	b3Scalar m_contactCFM2;
+
+	bool m_lateralFrictionInitialized;
+
+	b3Vector3 getPositionWorldOnA()
 	{
 		return m_positionWorldOnA;
 	}
-	b3Vector3	getPositionWorldOnB()
+	b3Vector3 getPositionWorldOnB()
 	{
 		return m_positionWorldOnB;
 	}
-	b3Scalar	getDistance()
+	b3Scalar getDistance()
 	{
 		return m_distance;
 	}
 };
 
-void	getContactPoint(b3Contact4* contact, int contactIndex, b3ContactPoint& pointOut)
+void getContactPoint(b3Contact4* contact, int contactIndex, b3ContactPoint& pointOut)
 {
 	pointOut.m_appliedImpulse = 0.f;
 	pointOut.m_appliedImpulseLateral1 = 0.f;
@@ -117,160 +113,145 @@ void	getContactPoint(b3Contact4* contact, int contactIndex, b3ContactPoint& poin
 	pointOut.m_contactCFM2 = 0.f;
 	pointOut.m_contactMotion1 = 0.f;
 	pointOut.m_contactMotion2 = 0.f;
-	pointOut.m_distance = contact->getPenetration(contactIndex);//??0.01f
+	pointOut.m_distance = contact->getPenetration(contactIndex);  //??0.01f
 	b3Vector3 normalOnB = contact->m_worldNormalOnB;
-	normalOnB.normalize();//is this needed?
+	normalOnB.normalize();  //is this needed?
 
-	b3Vector3 l1,l2;
-	b3PlaneSpace1(normalOnB,l1,l2);
+	b3Vector3 l1, l2;
+	b3PlaneSpace1(normalOnB, l1, l2);
 
 	pointOut.m_normalWorldOnB = normalOnB;
 	//printf("normalOnB = %f,%f,%f\n",normalOnB.getX(),normalOnB.getY(),normalOnB.getZ());
 	pointOut.m_lateralFrictionDir1 = l1;
 	pointOut.m_lateralFrictionDir2 = l2;
 	pointOut.m_lateralFrictionInitialized = true;
-	
-	
+
 	b3Vector3 worldPosB = contact->m_worldPosB[contactIndex];
 	pointOut.m_positionWorldOnB = worldPosB;
-	pointOut.m_positionWorldOnA = worldPosB+normalOnB*pointOut.m_distance;
+	pointOut.m_positionWorldOnA = worldPosB + normalOnB * pointOut.m_distance;
 }
 
-int	getNumContacts(b3Contact4* contact)
+int getNumContacts(b3Contact4* contact)
 {
 	return contact->getNPoints();
 }
 
 b3PgsJacobiSolver::b3PgsJacobiSolver(bool usePgs)
-:m_usePgs(usePgs),
-m_numSplitImpulseRecoveries(0),
-m_btSeed2(0)
+	: m_usePgs(usePgs),
+	  m_numSplitImpulseRecoveries(0),
+	  m_btSeed2(0)
 {
-
 }
 
 b3PgsJacobiSolver::~b3PgsJacobiSolver()
 {
 }
 
-void	b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints)
+void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints)
 {
 	b3ContactSolverInfo infoGlobal;
 	infoGlobal.m_splitImpulse = false;
-	infoGlobal.m_timeStep = 1.f/60.f;
-	infoGlobal.m_numIterations = 4;//4;
-//	infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS|B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION;
+	infoGlobal.m_timeStep = 1.f / 60.f;
+	infoGlobal.m_numIterations = 4;  //4;
+									 //	infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS|B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION;
 	//infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS;
-	infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS;
+	infoGlobal.m_solverMode |= B3_SOLVER_USE_2_FRICTION_DIRECTIONS;
 
 	//if (infoGlobal.m_solverMode & B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
 	//if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
-				
 
-	solveGroup(bodies,inertias,numBodies,contacts,numContacts,constraints,numConstraints,infoGlobal);
+	solveGroup(bodies, inertias, numBodies, contacts, numContacts, constraints, numConstraints, infoGlobal);
 
 	if (!numContacts)
 		return;
 }
 
-
-
-
 /// b3PgsJacobiSolver Sequentially applies impulses
 b3Scalar b3PgsJacobiSolver::solveGroup(b3RigidBodyData* bodies,
-										b3InertiaData* inertias, 
-										int numBodies,
-										b3Contact4* manifoldPtr, 
-										int numManifolds,
-										b3TypedConstraint** constraints,
-										int numConstraints,
-										const b3ContactSolverInfo& infoGlobal)
+									   b3InertiaData* inertias,
+									   int numBodies,
+									   b3Contact4* manifoldPtr,
+									   int numManifolds,
+									   b3TypedConstraint** constraints,
+									   int numConstraints,
+									   const b3ContactSolverInfo& infoGlobal)
 {
-
 	B3_PROFILE("solveGroup");
 	//you need to provide at least some bodies
-	
-	solveGroupCacheFriendlySetup( bodies, inertias,numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal);
-
-	solveGroupCacheFriendlyIterations(constraints, numConstraints,infoGlobal);
-
-	solveGroupCacheFriendlyFinish(bodies, inertias,numBodies, infoGlobal);
-	
-	return 0.f;
-}
-
-
-
-
 
+	solveGroupCacheFriendlySetup(bodies, inertias, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal);
 
+	solveGroupCacheFriendlyIterations(constraints, numConstraints, infoGlobal);
 
+	solveGroupCacheFriendlyFinish(bodies, inertias, numBodies, infoGlobal);
 
+	return 0.f;
+}
 
 #ifdef USE_SIMD
 #include <emmintrin.h>
-#define b3VecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
-static inline __m128 b3SimdDot3( __m128 vec0, __m128 vec1 )
+#define b3VecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e, e, e, e))
+static inline __m128 b3SimdDot3(__m128 vec0, __m128 vec1)
 {
-	__m128 result = _mm_mul_ps( vec0, vec1);
-	return _mm_add_ps( b3VecSplat( result, 0 ), _mm_add_ps( b3VecSplat( result, 1 ), b3VecSplat( result, 2 ) ) );
+	__m128 result = _mm_mul_ps(vec0, vec1);
+	return _mm_add_ps(b3VecSplat(result, 0), _mm_add_ps(b3VecSplat(result, 1), b3VecSplat(result, 2)));
 }
-#endif//USE_SIMD
+#endif  //USE_SIMD
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
-void b3PgsJacobiSolver::resolveSingleConstraintRowGenericSIMD(b3SolverBody& body1,b3SolverBody& body2,const b3SolverConstraint& c)
+void b3PgsJacobiSolver::resolveSingleConstraintRowGenericSIMD(b3SolverBody& body1, b3SolverBody& body2, const b3SolverConstraint& c)
 {
 #ifdef USE_SIMD
 	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
-	__m128	lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
-	__m128	upperLimit1 = _mm_set1_ps(c.m_upperLimit);
-	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
-	__m128 deltaVel1Dotn	=	_mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
-	__m128 deltaVel2Dotn	=	_mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),b3SimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
-	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
-	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
-	b3SimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
-	b3SimdScalar resultLowerLess,resultUpperLess;
-	resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
-	resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
-	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
-	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
-	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
-	__m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
-	deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
-	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
-	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
-	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
+	__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+	__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm)));
+	__m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
+	__m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128), b3SimdDot3((c.m_contactNormal).mVec128, body2.internalGetDeltaLinearVelocity().mVec128));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	b3SimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
+	b3SimdScalar resultLowerLess, resultUpperLess;
+	resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
+	resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
+	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
+	deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
+	c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
+	__m128 upperMinApplied = _mm_sub_ps(upperLimit1, cpAppliedImp);
+	deltaImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied));
+	c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1));
+	__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128, body1.internalGetInvMass().mVec128);
+	__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128, body2.internalGetInvMass().mVec128);
 	__m128 impulseMagnitude = deltaImpulse;
-	body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
-	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
-	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
-	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+	body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
+	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
+	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
+	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
 #else
-	resolveSingleConstraintRowGeneric(body1,body2,c);
+	resolveSingleConstraintRowGeneric(body1, body2, c);
 #endif
 }
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
- void b3PgsJacobiSolver::resolveSingleConstraintRowGeneric(b3SolverBody& body1,b3SolverBody& body2,const b3SolverConstraint& c)
+void b3PgsJacobiSolver::resolveSingleConstraintRowGeneric(b3SolverBody& body1, b3SolverBody& body2, const b3SolverConstraint& c)
 {
-	b3Scalar deltaImpulse = c.m_rhs-b3Scalar(c.m_appliedImpulse)*c.m_cfm;
-	const b3Scalar deltaVel1Dotn	=	c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
-	const b3Scalar deltaVel2Dotn	=	-c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
+	b3Scalar deltaImpulse = c.m_rhs - b3Scalar(c.m_appliedImpulse) * c.m_cfm;
+	const b3Scalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
+	const b3Scalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
 
-//	const b3Scalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;
-	deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
-	deltaImpulse	-=	deltaVel2Dotn*c.m_jacDiagABInv;
+	//	const b3Scalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;
+	deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
+	deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
 
 	const b3Scalar sum = b3Scalar(c.m_appliedImpulse) + deltaImpulse;
 	if (sum < c.m_lowerLimit)
 	{
-		deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
+		deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
 		c.m_appliedImpulse = c.m_lowerLimit;
 	}
-	else if (sum > c.m_upperLimit) 
+	else if (sum > c.m_upperLimit)
 	{
-		deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
+		deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
 		c.m_appliedImpulse = c.m_upperLimit;
 	}
 	else
@@ -278,94 +259,93 @@ void b3PgsJacobiSolver::resolveSingleConstraintRowGenericSIMD(b3SolverBody& body
 		c.m_appliedImpulse = sum;
 	}
 
-	body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
-	body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+	body1.internalApplyImpulse(c.m_contactNormal * body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+	body2.internalApplyImpulse(-c.m_contactNormal * body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
 }
 
- void b3PgsJacobiSolver::resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& body1,b3SolverBody& body2,const b3SolverConstraint& c)
+void b3PgsJacobiSolver::resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& body1, b3SolverBody& body2, const b3SolverConstraint& c)
 {
 #ifdef USE_SIMD
 	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
-	__m128	lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
-	__m128	upperLimit1 = _mm_set1_ps(c.m_upperLimit);
-	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
-	__m128 deltaVel1Dotn	=	_mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
-	__m128 deltaVel2Dotn	=	_mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),b3SimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
-	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
-	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
-	b3SimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
-	b3SimdScalar resultLowerLess,resultUpperLess;
-	resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
-	resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
-	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
-	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
-	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
-	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
-	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
+	__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+	__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm)));
+	__m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
+	__m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128), b3SimdDot3((c.m_contactNormal).mVec128, body2.internalGetDeltaLinearVelocity().mVec128));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	b3SimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
+	b3SimdScalar resultLowerLess, resultUpperLess;
+	resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
+	resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
+	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
+	deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
+	c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
+	__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128, body1.internalGetInvMass().mVec128);
+	__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128, body2.internalGetInvMass().mVec128);
 	__m128 impulseMagnitude = deltaImpulse;
-	body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
-	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
-	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
-	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+	body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
+	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
+	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
+	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
 #else
-	resolveSingleConstraintRowLowerLimit(body1,body2,c);
+	resolveSingleConstraintRowLowerLimit(body1, body2, c);
 #endif
 }
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
- void b3PgsJacobiSolver::resolveSingleConstraintRowLowerLimit(b3SolverBody& body1,b3SolverBody& body2,const b3SolverConstraint& c)
+void b3PgsJacobiSolver::resolveSingleConstraintRowLowerLimit(b3SolverBody& body1, b3SolverBody& body2, const b3SolverConstraint& c)
 {
-	b3Scalar deltaImpulse = c.m_rhs-b3Scalar(c.m_appliedImpulse)*c.m_cfm;
-	const b3Scalar deltaVel1Dotn	=	c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
-	const b3Scalar deltaVel2Dotn	=	-c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
+	b3Scalar deltaImpulse = c.m_rhs - b3Scalar(c.m_appliedImpulse) * c.m_cfm;
+	const b3Scalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
+	const b3Scalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
 
-	deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
-	deltaImpulse	-=	deltaVel2Dotn*c.m_jacDiagABInv;
+	deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
+	deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
 	const b3Scalar sum = b3Scalar(c.m_appliedImpulse) + deltaImpulse;
 	if (sum < c.m_lowerLimit)
 	{
-		deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
+		deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
 		c.m_appliedImpulse = c.m_lowerLimit;
 	}
 	else
 	{
 		c.m_appliedImpulse = sum;
 	}
-	body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
-	body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+	body1.internalApplyImpulse(c.m_contactNormal * body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+	body2.internalApplyImpulse(-c.m_contactNormal * body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
 }
 
-
-void	b3PgsJacobiSolver::resolveSplitPenetrationImpulseCacheFriendly(
-        b3SolverBody& body1,
-        b3SolverBody& body2,
-        const b3SolverConstraint& c)
+void b3PgsJacobiSolver::resolveSplitPenetrationImpulseCacheFriendly(
+	b3SolverBody& body1,
+	b3SolverBody& body2,
+	const b3SolverConstraint& c)
 {
-		if (c.m_rhsPenetration)
-        {
-			m_numSplitImpulseRecoveries++;
-			b3Scalar deltaImpulse = c.m_rhsPenetration-b3Scalar(c.m_appliedPushImpulse)*c.m_cfm;
-			const b3Scalar deltaVel1Dotn	=	c.m_contactNormal.dot(body1.internalGetPushVelocity()) 	+ c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
-			const b3Scalar deltaVel2Dotn	=	-c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
-
-			deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
-			deltaImpulse	-=	deltaVel2Dotn*c.m_jacDiagABInv;
-			const b3Scalar sum = b3Scalar(c.m_appliedPushImpulse) + deltaImpulse;
-			if (sum < c.m_lowerLimit)
-			{
-				deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
-				c.m_appliedPushImpulse = c.m_lowerLimit;
-			}
-			else
-			{
-				c.m_appliedPushImpulse = sum;
-			}
-			body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
-			body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
-        }
+	if (c.m_rhsPenetration)
+	{
+		m_numSplitImpulseRecoveries++;
+		b3Scalar deltaImpulse = c.m_rhsPenetration - b3Scalar(c.m_appliedPushImpulse) * c.m_cfm;
+		const b3Scalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
+		const b3Scalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
+
+		deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
+		deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
+		const b3Scalar sum = b3Scalar(c.m_appliedPushImpulse) + deltaImpulse;
+		if (sum < c.m_lowerLimit)
+		{
+			deltaImpulse = c.m_lowerLimit - c.m_appliedPushImpulse;
+			c.m_appliedPushImpulse = c.m_lowerLimit;
+		}
+		else
+		{
+			c.m_appliedPushImpulse = sum;
+		}
+		body1.internalApplyPushImpulse(c.m_contactNormal * body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+		body2.internalApplyPushImpulse(-c.m_contactNormal * body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
+	}
 }
 
- void b3PgsJacobiSolver::resolveSplitPenetrationSIMD(b3SolverBody& body1,b3SolverBody& body2,const b3SolverConstraint& c)
+void b3PgsJacobiSolver::resolveSplitPenetrationSIMD(b3SolverBody& body1, b3SolverBody& body2, const b3SolverConstraint& c)
 {
 #ifdef USE_SIMD
 	if (!c.m_rhsPenetration)
@@ -374,44 +354,40 @@ void	b3PgsJacobiSolver::resolveSplitPenetrationImpulseCacheFriendly(
 	m_numSplitImpulseRecoveries++;
 
 	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
-	__m128	lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
-	__m128	upperLimit1 = _mm_set1_ps(c.m_upperLimit);
-	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm)));
-	__m128 deltaVel1Dotn	=	_mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
-	__m128 deltaVel2Dotn	=	_mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),b3SimdDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128));
-	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
-	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
-	b3SimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
-	b3SimdScalar resultLowerLess,resultUpperLess;
-	resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
-	resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
-	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
-	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
-	c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
-	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
-	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
+	__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+	__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse), _mm_set1_ps(c.m_cfm)));
+	__m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128, body1.internalGetPushVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetTurnVelocity().mVec128));
+	__m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetTurnVelocity().mVec128), b3SimdDot3((c.m_contactNormal).mVec128, body2.internalGetPushVelocity().mVec128));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
+	b3SimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
+	b3SimdScalar resultLowerLess, resultUpperLess;
+	resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
+	resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
+	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
+	deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
+	c.m_appliedPushImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
+	__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128, body1.internalGetInvMass().mVec128);
+	__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128, body2.internalGetInvMass().mVec128);
 	__m128 impulseMagnitude = deltaImpulse;
-	body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
-	body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
-	body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
-	body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+	body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
+	body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
+	body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
+	body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
 #else
-	resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
+	resolveSplitPenetrationImpulseCacheFriendly(body1, body2, c);
 #endif
 }
 
-
-
 unsigned long b3PgsJacobiSolver::b3Rand2()
 {
-	m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
+	m_btSeed2 = (1664525L * m_btSeed2 + 1013904223L) & 0xffffffff;
 	return m_btSeed2;
 }
 
-
-
 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
-int b3PgsJacobiSolver::b3RandInt2 (int n)
+int b3PgsJacobiSolver::b3RandInt2(int n)
 {
 	// seems good; xor-fold and modulus
 	const unsigned long un = static_cast<unsigned long>(n);
@@ -419,15 +395,20 @@ int b3PgsJacobiSolver::b3RandInt2 (int n)
 
 	// note: probably more aggressive than it needs to be -- might be
 	//       able to get away without one or two of the innermost branches.
-	if (un <= 0x00010000UL) {
+	if (un <= 0x00010000UL)
+	{
 		r ^= (r >> 16);
-		if (un <= 0x00000100UL) {
+		if (un <= 0x00000100UL)
+		{
 			r ^= (r >> 8);
-			if (un <= 0x00000010UL) {
+			if (un <= 0x00000010UL)
+			{
 				r ^= (r >> 4);
-				if (un <= 0x00000004UL) {
+				if (un <= 0x00000004UL)
+				{
 					r ^= (r >> 2);
-					if (un <= 0x00000002UL) {
+					if (un <= 0x00000002UL)
+					{
 						r ^= (r >> 1);
 					}
 				}
@@ -435,62 +416,46 @@ int b3PgsJacobiSolver::b3RandInt2 (int n)
 		}
 	}
 
-	return (int) (r % un);
+	return (int)(r % un);
 }
 
-
-
-void	b3PgsJacobiSolver::initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* rb)
+void b3PgsJacobiSolver::initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* rb)
 {
-
-	solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
-	solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
-	solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
-	solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
+	solverBody->m_deltaLinearVelocity.setValue(0.f, 0.f, 0.f);
+	solverBody->m_deltaAngularVelocity.setValue(0.f, 0.f, 0.f);
+	solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
+	solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
 
 	if (rb)
 	{
 		solverBody->m_worldTransform = getWorldTransform(rb);
-		solverBody->internalSetInvMass(b3MakeVector3(rb->m_invMass,rb->m_invMass,rb->m_invMass));
+		solverBody->internalSetInvMass(b3MakeVector3(rb->m_invMass, rb->m_invMass, rb->m_invMass));
 		solverBody->m_originalBodyIndex = bodyIndex;
-		solverBody->m_angularFactor = b3MakeVector3(1,1,1);
-		solverBody->m_linearFactor = b3MakeVector3(1,1,1);
+		solverBody->m_angularFactor = b3MakeVector3(1, 1, 1);
+		solverBody->m_linearFactor = b3MakeVector3(1, 1, 1);
 		solverBody->m_linearVelocity = getLinearVelocity(rb);
 		solverBody->m_angularVelocity = getAngularVelocity(rb);
-	} else
+	}
+	else
 	{
 		solverBody->m_worldTransform.setIdentity();
-		solverBody->internalSetInvMass(b3MakeVector3(0,0,0));
+		solverBody->internalSetInvMass(b3MakeVector3(0, 0, 0));
 		solverBody->m_originalBodyIndex = bodyIndex;
-		solverBody->m_angularFactor.setValue(1,1,1);
-		solverBody->m_linearFactor.setValue(1,1,1);
-		solverBody->m_linearVelocity.setValue(0,0,0);
-		solverBody->m_angularVelocity.setValue(0,0,0);
+		solverBody->m_angularFactor.setValue(1, 1, 1);
+		solverBody->m_linearFactor.setValue(1, 1, 1);
+		solverBody->m_linearVelocity.setValue(0, 0, 0);
+		solverBody->m_angularVelocity.setValue(0, 0, 0);
 	}
-
-
 }
 
-
-
-
-
-
 b3Scalar b3PgsJacobiSolver::restitutionCurve(b3Scalar rel_vel, b3Scalar restitution)
 {
 	b3Scalar rest = restitution * -rel_vel;
 	return rest;
 }
 
-
-
-
-
-
-void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int  solverBodyIdA,int solverBodyIdB,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
+void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
 {
-
-	
 	solverConstraint.m_contactNormal = normalAxis;
 	b3SolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
 	b3SolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
@@ -498,7 +463,6 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies,b3Inerti
 	b3RigidBodyData* body0 = &bodies[solverBodyA.m_originalBodyIndex];
 	b3RigidBodyData* body1 = &bodies[solverBodyB.m_originalBodyIndex];
 
-
 	solverConstraint.m_solverBodyIdA = solverBodyIdA;
 	solverConstraint.m_solverBodyIdB = solverBodyIdB;
 
@@ -511,12 +475,12 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies,b3Inerti
 	{
 		b3Vector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
 		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentA = body0 ? getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex])*ftorqueAxis1 : b3MakeVector3(0,0,0);
+		solverConstraint.m_angularComponentA = body0 ? getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex]) * ftorqueAxis1 : b3MakeVector3(0, 0, 0);
 	}
 	{
 		b3Vector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
 		solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentB = body1 ? getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex])*ftorqueAxis1 : b3MakeVector3(0,0,0);
+		solverConstraint.m_angularComponentB = body1 ? getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex]) * ftorqueAxis1 : b3MakeVector3(0, 0, 0);
 	}
 
 	b3Scalar scaledDenom;
@@ -527,72 +491,66 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies,b3Inerti
 		b3Scalar denom1 = 0.f;
 		if (body0)
 		{
-			vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+			vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
 			denom0 = body0->m_invMass + normalAxis.dot(vec);
 		}
 		if (body1)
 		{
-			vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+			vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
 			denom1 = body1->m_invMass + normalAxis.dot(vec);
 		}
 
 		b3Scalar denom;
 		if (m_usePgs)
 		{
-			scaledDenom = denom = relaxation/(denom0+denom1);
-		} else
+			scaledDenom = denom = relaxation / (denom0 + denom1);
+		}
+		else
 		{
-			denom = relaxation/(denom0+denom1);
-			b3Scalar countA = body0->m_invMass ? b3Scalar(m_bodyCount[solverBodyA.m_originalBodyIndex]): 1.f;
-			b3Scalar countB = body1->m_invMass ? b3Scalar(m_bodyCount[solverBodyB.m_originalBodyIndex]): 1.f;
+			denom = relaxation / (denom0 + denom1);
+			b3Scalar countA = body0->m_invMass ? b3Scalar(m_bodyCount[solverBodyA.m_originalBodyIndex]) : 1.f;
+			b3Scalar countB = body1->m_invMass ? b3Scalar(m_bodyCount[solverBodyB.m_originalBodyIndex]) : 1.f;
 
-			scaledDenom = relaxation/(denom0*countA+denom1*countB);
+			scaledDenom = relaxation / (denom0 * countA + denom1 * countB);
 		}
 
 		solverConstraint.m_jacDiagABInv = denom;
 	}
 
 	{
-		
-
 		b3Scalar rel_vel;
-		b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:b3MakeVector3(0,0,0)) 
-			+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:b3MakeVector3(0,0,0));
-		b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:b3MakeVector3(0,0,0)) 
-			+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:b3MakeVector3(0,0,0));
+		b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0 ? solverBodyA.m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : b3MakeVector3(0, 0, 0));
+		b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1 ? solverBodyB.m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(body1 ? solverBodyB.m_angularVelocity : b3MakeVector3(0, 0, 0));
 
-		rel_vel = vel1Dotn+vel2Dotn;
+		rel_vel = vel1Dotn + vel2Dotn;
 
-//		b3Scalar positionalError = 0.f;
+		//		b3Scalar positionalError = 0.f;
 
-		b3SimdScalar velocityError =  desiredVelocity - rel_vel;
-		b3SimdScalar	velocityImpulse = velocityError * b3SimdScalar(scaledDenom);//solverConstraint.m_jacDiagABInv);
+		b3SimdScalar velocityError = desiredVelocity - rel_vel;
+		b3SimdScalar velocityImpulse = velocityError * b3SimdScalar(scaledDenom);  //solverConstraint.m_jacDiagABInv);
 		solverConstraint.m_rhs = velocityImpulse;
 		solverConstraint.m_cfm = cfmSlip;
 		solverConstraint.m_lowerLimit = 0;
 		solverConstraint.m_upperLimit = 1e10f;
-		
 	}
 }
 
-b3SolverConstraint&	b3PgsJacobiSolver::addFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
+b3SolverConstraint& b3PgsJacobiSolver::addFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
 {
 	b3SolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
 	solverConstraint.m_frictionIndex = frictionIndex;
-	setupFrictionConstraint(bodies,inertias,solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, 
+	setupFrictionConstraint(bodies, inertias, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
 							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
 
-
-void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,	b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis1,int solverBodyIdA,int  solverBodyIdB,
-									b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
-									b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, 
-									b3Scalar desiredVelocity, b3Scalar cfmSlip)
+void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis1, int solverBodyIdA, int solverBodyIdB,
+													   b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
+													   b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
+													   b3Scalar desiredVelocity, b3Scalar cfmSlip)
 
 {
-	b3Vector3 normalAxis=b3MakeVector3(0,0,0);
-
+	b3Vector3 normalAxis = b3MakeVector3(0, 0, 0);
 
 	solverConstraint.m_contactNormal = normalAxis;
 	b3SolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
@@ -613,283 +571,256 @@ void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyData* bodies,b
 	{
 		b3Vector3 ftorqueAxis1 = -normalAxis1;
 		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentA = body0 ? getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex])*ftorqueAxis1 : b3MakeVector3(0,0,0);
+		solverConstraint.m_angularComponentA = body0 ? getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex]) * ftorqueAxis1 : b3MakeVector3(0, 0, 0);
 	}
 	{
 		b3Vector3 ftorqueAxis1 = normalAxis1;
 		solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentB = body1 ? getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex])*ftorqueAxis1 : b3MakeVector3(0,0,0);
+		solverConstraint.m_angularComponentB = body1 ? getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex]) * ftorqueAxis1 : b3MakeVector3(0, 0, 0);
 	}
 
-
 	{
-		b3Vector3 iMJaA = body0?getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex])*solverConstraint.m_relpos1CrossNormal:b3MakeVector3(0,0,0);
-		b3Vector3 iMJaB = body1?getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex])*solverConstraint.m_relpos2CrossNormal:b3MakeVector3(0,0,0);
+		b3Vector3 iMJaA = body0 ? getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex]) * solverConstraint.m_relpos1CrossNormal : b3MakeVector3(0, 0, 0);
+		b3Vector3 iMJaB = body1 ? getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex]) * solverConstraint.m_relpos2CrossNormal : b3MakeVector3(0, 0, 0);
 		b3Scalar sum = 0;
 		sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
 		sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
-		solverConstraint.m_jacDiagABInv = b3Scalar(1.)/sum;
+		solverConstraint.m_jacDiagABInv = b3Scalar(1.) / sum;
 	}
 
 	{
-		
-
 		b3Scalar rel_vel;
-		b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:b3MakeVector3(0,0,0)) 
-			+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:b3MakeVector3(0,0,0));
-		b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:b3MakeVector3(0,0,0)) 
-			+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:b3MakeVector3(0,0,0));
+		b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0 ? solverBodyA.m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : b3MakeVector3(0, 0, 0));
+		b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1 ? solverBodyB.m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(body1 ? solverBodyB.m_angularVelocity : b3MakeVector3(0, 0, 0));
 
-		rel_vel = vel1Dotn+vel2Dotn;
+		rel_vel = vel1Dotn + vel2Dotn;
 
-//		b3Scalar positionalError = 0.f;
+		//		b3Scalar positionalError = 0.f;
 
-		b3SimdScalar velocityError =  desiredVelocity - rel_vel;
-		b3SimdScalar	velocityImpulse = velocityError * b3SimdScalar(solverConstraint.m_jacDiagABInv);
+		b3SimdScalar velocityError = desiredVelocity - rel_vel;
+		b3SimdScalar velocityImpulse = velocityError * b3SimdScalar(solverConstraint.m_jacDiagABInv);
 		solverConstraint.m_rhs = velocityImpulse;
 		solverConstraint.m_cfm = cfmSlip;
 		solverConstraint.m_lowerLimit = 0;
 		solverConstraint.m_upperLimit = 1e10f;
-		
 	}
 }
 
-
-
-
-
-
-
-
-b3SolverConstraint&	b3PgsJacobiSolver::addRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
+b3SolverConstraint& b3PgsJacobiSolver::addRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
 {
 	b3SolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
 	solverConstraint.m_frictionIndex = frictionIndex;
-	setupRollingFrictionConstraint(bodies,inertias,solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, 
-							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
+	setupRollingFrictionConstraint(bodies, inertias, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
+								   colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
 
-
-int	b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies,b3InertiaData* inertias)
+int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies, b3InertiaData* inertias)
 {
 	//b3Assert(bodyIndex< m_tmpSolverBodyPool.size());
 
 	b3RigidBodyData& body = bodies[bodyIndex];
 	int curIndex = -1;
-	if (m_usePgs || body.m_invMass==0.f)
+	if (m_usePgs || body.m_invMass == 0.f)
 	{
-		if (m_bodyCount[bodyIndex]<0)
+		if (m_bodyCount[bodyIndex] < 0)
 		{
 			curIndex = m_tmpSolverBodyPool.size();
 			b3SolverBody& solverBody = m_tmpSolverBodyPool.expand();
-			initSolverBody(bodyIndex,&solverBody,&body);
+			initSolverBody(bodyIndex, &solverBody, &body);
 			solverBody.m_originalBodyIndex = bodyIndex;
 			m_bodyCount[bodyIndex] = curIndex;
-		} else
+		}
+		else
 		{
 			curIndex = m_bodyCount[bodyIndex];
 		}
-	} else
+	}
+	else
 	{
-		b3Assert(m_bodyCount[bodyIndex]>0);
+		b3Assert(m_bodyCount[bodyIndex] > 0);
 		m_bodyCountCheck[bodyIndex]++;
 		curIndex = m_tmpSolverBodyPool.size();
 		b3SolverBody& solverBody = m_tmpSolverBodyPool.expand();
-		initSolverBody(bodyIndex,&solverBody,&body);
+		initSolverBody(bodyIndex, &solverBody, &body);
 		solverBody.m_originalBodyIndex = bodyIndex;
 	}
 
-	b3Assert(curIndex>=0);
+	b3Assert(curIndex >= 0);
 	return curIndex;
-
 }
 #include <stdio.h>
 
-
-void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias,b3SolverConstraint& solverConstraint, 
-																 int solverBodyIdA, int solverBodyIdB,
-																 b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal,
-																 b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
-																 b3Vector3& rel_pos1, b3Vector3& rel_pos2)
+void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint,
+											   int solverBodyIdA, int solverBodyIdB,
+											   b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal,
+											   b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
+											   b3Vector3& rel_pos1, b3Vector3& rel_pos2)
 {
-			
-			const b3Vector3& pos1 = cp.getPositionWorldOnA();
-			const b3Vector3& pos2 = cp.getPositionWorldOnB();
+	const b3Vector3& pos1 = cp.getPositionWorldOnA();
+	const b3Vector3& pos2 = cp.getPositionWorldOnB();
 
-			b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
-			b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
+	b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
+	b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
 
-			b3RigidBodyData* rb0 = &bodies[bodyA->m_originalBodyIndex];
-			b3RigidBodyData* rb1 = &bodies[bodyB->m_originalBodyIndex];
+	b3RigidBodyData* rb0 = &bodies[bodyA->m_originalBodyIndex];
+	b3RigidBodyData* rb1 = &bodies[bodyB->m_originalBodyIndex];
 
-//			b3Vector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
-//			b3Vector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
-			rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
-			rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
+	//			b3Vector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
+	//			b3Vector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
+	rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
+	rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
 
-			relaxation = 1.f;
+	relaxation = 1.f;
 
-			b3Vector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
-			solverConstraint.m_angularComponentA = rb0 ? getInvInertiaTensorWorld(&inertias[bodyA->m_originalBodyIndex])*torqueAxis0 : b3MakeVector3(0,0,0);
-			b3Vector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
-			solverConstraint.m_angularComponentB = rb1 ? getInvInertiaTensorWorld(&inertias[bodyB->m_originalBodyIndex])*-torqueAxis1 : b3MakeVector3(0,0,0);
+	b3Vector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+	solverConstraint.m_angularComponentA = rb0 ? getInvInertiaTensorWorld(&inertias[bodyA->m_originalBodyIndex]) * torqueAxis0 : b3MakeVector3(0, 0, 0);
+	b3Vector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
+	solverConstraint.m_angularComponentB = rb1 ? getInvInertiaTensorWorld(&inertias[bodyB->m_originalBodyIndex]) * -torqueAxis1 : b3MakeVector3(0, 0, 0);
 
-			b3Scalar scaledDenom;
-				{
+	b3Scalar scaledDenom;
+	{
 #ifdef COMPUTE_IMPULSE_DENOM
-					b3Scalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
-					b3Scalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
-#else							
-					b3Vector3 vec;
-					b3Scalar denom0 = 0.f;
-					b3Scalar denom1 = 0.f;
-					if (rb0)
-					{
-						vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
-						denom0 = rb0->m_invMass + cp.m_normalWorldOnB.dot(vec);
-					}
-					if (rb1)
-					{
-						vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
-						denom1 = rb1->m_invMass + cp.m_normalWorldOnB.dot(vec);
-					}
-#endif //COMPUTE_IMPULSE_DENOM		
-
-					
-					b3Scalar denom;
-					if (m_usePgs)
-					{
-						scaledDenom = denom = relaxation/(denom0+denom1);
-					} else
-					{
-						denom = relaxation/(denom0+denom1);
-
-						b3Scalar countA = rb0->m_invMass? b3Scalar(m_bodyCount[bodyA->m_originalBodyIndex]) : 1.f;
-						b3Scalar countB = rb1->m_invMass? b3Scalar(m_bodyCount[bodyB->m_originalBodyIndex]) : 1.f;
-						scaledDenom = relaxation/(denom0*countA+denom1*countB);
-					}
-					solverConstraint.m_jacDiagABInv = denom;
-				}
-
-				solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
-				solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-				solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
-
-				b3Scalar restitution = 0.f;
-				b3Scalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
-
-				{
-					b3Vector3 vel1,vel2;
+		b3Scalar denom0 = rb0->computeImpulseDenominator(pos1, cp.m_normalWorldOnB);
+		b3Scalar denom1 = rb1->computeImpulseDenominator(pos2, cp.m_normalWorldOnB);
+#else
+		b3Vector3 vec;
+		b3Scalar denom0 = 0.f;
+		b3Scalar denom1 = 0.f;
+		if (rb0)
+		{
+			vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
+			denom0 = rb0->m_invMass + cp.m_normalWorldOnB.dot(vec);
+		}
+		if (rb1)
+		{
+			vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
+			denom1 = rb1->m_invMass + cp.m_normalWorldOnB.dot(vec);
+		}
+#endif  //COMPUTE_IMPULSE_DENOM
 
-					vel1 = rb0? getVelocityInLocalPoint(rb0,rel_pos1) : b3MakeVector3(0,0,0);
-					vel2 = rb1? getVelocityInLocalPoint(rb1, rel_pos2) : b3MakeVector3(0,0,0);
+		b3Scalar denom;
+		if (m_usePgs)
+		{
+			scaledDenom = denom = relaxation / (denom0 + denom1);
+		}
+		else
+		{
+			denom = relaxation / (denom0 + denom1);
 
-	//			b3Vector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : b3Vector3(0,0,0);
-					vel  = vel1 - vel2;
-					rel_vel = cp.m_normalWorldOnB.dot(vel);
+			b3Scalar countA = rb0->m_invMass ? b3Scalar(m_bodyCount[bodyA->m_originalBodyIndex]) : 1.f;
+			b3Scalar countB = rb1->m_invMass ? b3Scalar(m_bodyCount[bodyB->m_originalBodyIndex]) : 1.f;
+			scaledDenom = relaxation / (denom0 * countA + denom1 * countB);
+		}
+		solverConstraint.m_jacDiagABInv = denom;
+	}
 
-					
+	solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
+	solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+	solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
 
-					solverConstraint.m_friction = cp.m_combinedFriction;
+	b3Scalar restitution = 0.f;
+	b3Scalar penetration = cp.getDistance() + infoGlobal.m_linearSlop;
 
-				
-					restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
-					if (restitution <= b3Scalar(0.))
-					{
-						restitution = 0.f;
-					};
-				}
+	{
+		b3Vector3 vel1, vel2;
 
+		vel1 = rb0 ? getVelocityInLocalPoint(rb0, rel_pos1) : b3MakeVector3(0, 0, 0);
+		vel2 = rb1 ? getVelocityInLocalPoint(rb1, rel_pos2) : b3MakeVector3(0, 0, 0);
 
-				///warm starting (or zero if disabled)
-				if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING)
-				{
-					solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-					if (rb0)
-						bodyA->internalApplyImpulse(solverConstraint.m_contactNormal*bodyA->internalGetInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
-					if (rb1)
-						bodyB->internalApplyImpulse(solverConstraint.m_contactNormal*bodyB->internalGetInvMass(),-solverConstraint.m_angularComponentB,-(b3Scalar)solverConstraint.m_appliedImpulse);
-				} else
-				{
-					solverConstraint.m_appliedImpulse = 0.f;
-				}
+		//			b3Vector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : b3Vector3(0,0,0);
+		vel = vel1 - vel2;
+		rel_vel = cp.m_normalWorldOnB.dot(vel);
 
-				solverConstraint.m_appliedPushImpulse = 0.f;
+		solverConstraint.m_friction = cp.m_combinedFriction;
 
-				{
-					b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?bodyA->m_linearVelocity:b3MakeVector3(0,0,0)) 
-						+ solverConstraint.m_relpos1CrossNormal.dot(rb0?bodyA->m_angularVelocity:b3MakeVector3(0,0,0));
-					b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?bodyB->m_linearVelocity:b3MakeVector3(0,0,0)) 
-						+ solverConstraint.m_relpos2CrossNormal.dot(rb1?bodyB->m_angularVelocity:b3MakeVector3(0,0,0));
-					b3Scalar rel_vel = vel1Dotn+vel2Dotn;
-
-					b3Scalar positionalError = 0.f;
-					b3Scalar	velocityError = restitution - rel_vel;// * damping;
-					
-
-					b3Scalar erp = infoGlobal.m_erp2;
-					if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-					{
-						erp = infoGlobal.m_erp;
-					}
+		restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
+		if (restitution <= b3Scalar(0.))
+		{
+			restitution = 0.f;
+		};
+	}
 
-					if (penetration>0)
-					{
-						positionalError = 0;
+	///warm starting (or zero if disabled)
+	if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING)
+	{
+		solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+		if (rb0)
+			bodyA->internalApplyImpulse(solverConstraint.m_contactNormal * bodyA->internalGetInvMass(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
+		if (rb1)
+			bodyB->internalApplyImpulse(solverConstraint.m_contactNormal * bodyB->internalGetInvMass(), -solverConstraint.m_angularComponentB, -(b3Scalar)solverConstraint.m_appliedImpulse);
+	}
+	else
+	{
+		solverConstraint.m_appliedImpulse = 0.f;
+	}
 
-						velocityError -= penetration / infoGlobal.m_timeStep;
-					} else
-					{
-						positionalError = -penetration * erp/infoGlobal.m_timeStep;
-					}
+	solverConstraint.m_appliedPushImpulse = 0.f;
 
-					b3Scalar  penetrationImpulse = positionalError*scaledDenom;//solverConstraint.m_jacDiagABInv;
-					b3Scalar velocityImpulse = velocityError *scaledDenom;//solverConstraint.m_jacDiagABInv;
+	{
+		b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0 ? bodyA->m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(rb0 ? bodyA->m_angularVelocity : b3MakeVector3(0, 0, 0));
+		b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1 ? bodyB->m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(rb1 ? bodyB->m_angularVelocity : b3MakeVector3(0, 0, 0));
+		b3Scalar rel_vel = vel1Dotn + vel2Dotn;
 
-					if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-					{
-						//combine position and velocity into rhs
-						solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
-						solverConstraint.m_rhsPenetration = 0.f;
+		b3Scalar positionalError = 0.f;
+		b3Scalar velocityError = restitution - rel_vel;  // * damping;
 
-					} else
-					{
-						//split position and velocity into rhs and m_rhsPenetration
-						solverConstraint.m_rhs = velocityImpulse;
-						solverConstraint.m_rhsPenetration = penetrationImpulse;
-					}
-					solverConstraint.m_cfm = 0.f;
-					solverConstraint.m_lowerLimit = 0;
-					solverConstraint.m_upperLimit = 1e10f;
-				}
+		b3Scalar erp = infoGlobal.m_erp2;
+		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		{
+			erp = infoGlobal.m_erp;
+		}
 
+		if (penetration > 0)
+		{
+			positionalError = 0;
 
+			velocityError -= penetration / infoGlobal.m_timeStep;
+		}
+		else
+		{
+			positionalError = -penetration * erp / infoGlobal.m_timeStep;
+		}
 
+		b3Scalar penetrationImpulse = positionalError * scaledDenom;  //solverConstraint.m_jacDiagABInv;
+		b3Scalar velocityImpulse = velocityError * scaledDenom;       //solverConstraint.m_jacDiagABInv;
 
+		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		{
+			//combine position and velocity into rhs
+			solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
+			solverConstraint.m_rhsPenetration = 0.f;
+		}
+		else
+		{
+			//split position and velocity into rhs and m_rhsPenetration
+			solverConstraint.m_rhs = velocityImpulse;
+			solverConstraint.m_rhsPenetration = penetrationImpulse;
+		}
+		solverConstraint.m_cfm = 0.f;
+		solverConstraint.m_lowerLimit = 0;
+		solverConstraint.m_upperLimit = 1e10f;
+	}
 }
 
-
-
-void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyData* bodies, b3InertiaData* inertias,b3SolverConstraint& solverConstraint, 
-																		int solverBodyIdA, int solverBodyIdB,
-																 b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal)
+void b3PgsJacobiSolver::setFrictionConstraintImpulse(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint,
+													 int solverBodyIdA, int solverBodyIdB,
+													 b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal)
 {
-
 	b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
 	b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
 
-
 	{
 		b3SolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
 		if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING)
 		{
 			frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
 			if (bodies[bodyA->m_originalBodyIndex].m_invMass)
-				bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal*bodies[bodyA->m_originalBodyIndex].m_invMass,frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
+				bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal * bodies[bodyA->m_originalBodyIndex].m_invMass, frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse);
 			if (bodies[bodyB->m_originalBodyIndex].m_invMass)
-				bodyB->internalApplyImpulse(frictionConstraint1.m_contactNormal*bodies[bodyB->m_originalBodyIndex].m_invMass,-frictionConstraint1.m_angularComponentB,-(b3Scalar)frictionConstraint1.m_appliedImpulse);
-		} else
+				bodyB->internalApplyImpulse(frictionConstraint1.m_contactNormal * bodies[bodyB->m_originalBodyIndex].m_invMass, -frictionConstraint1.m_angularComponentB, -(b3Scalar)frictionConstraint1.m_appliedImpulse);
+		}
+		else
 		{
 			frictionConstraint1.m_appliedImpulse = 0.f;
 		}
@@ -897,51 +828,45 @@ void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyData* bodies, b
 
 	if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
 	{
-		b3SolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
+		b3SolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
 		if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING)
 		{
-			frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2  * infoGlobal.m_warmstartingFactor;
+			frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
 			if (bodies[bodyA->m_originalBodyIndex].m_invMass)
-				bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal*bodies[bodyA->m_originalBodyIndex].m_invMass,frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
+				bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal * bodies[bodyA->m_originalBodyIndex].m_invMass, frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse);
 			if (bodies[bodyB->m_originalBodyIndex].m_invMass)
-				bodyB->internalApplyImpulse(frictionConstraint2.m_contactNormal*bodies[bodyB->m_originalBodyIndex].m_invMass,-frictionConstraint2.m_angularComponentB,-(b3Scalar)frictionConstraint2.m_appliedImpulse);
-		} else
+				bodyB->internalApplyImpulse(frictionConstraint2.m_contactNormal * bodies[bodyB->m_originalBodyIndex].m_invMass, -frictionConstraint2.m_angularComponentB, -(b3Scalar)frictionConstraint2.m_appliedImpulse);
+		}
+		else
 		{
 			frictionConstraint2.m_appliedImpulse = 0.f;
 		}
 	}
 }
 
-
-
-
-void	b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal)
+void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias, b3Contact4* manifold, const b3ContactSolverInfo& infoGlobal)
 {
-	b3RigidBodyData* colObj0=0,*colObj1=0;
+	b3RigidBodyData *colObj0 = 0, *colObj1 = 0;
 
-	
-	int solverBodyIdA = getOrInitSolverBody(manifold->getBodyA(),bodies,inertias);
-	int solverBodyIdB = getOrInitSolverBody(manifold->getBodyB(),bodies,inertias);
+	int solverBodyIdA = getOrInitSolverBody(manifold->getBodyA(), bodies, inertias);
+	int solverBodyIdB = getOrInitSolverBody(manifold->getBodyB(), bodies, inertias);
 
-//	b3RigidBody* bodyA = b3RigidBody::upcast(colObj0);
-//	b3RigidBody* bodyB = b3RigidBody::upcast(colObj1);
+	//	b3RigidBody* bodyA = b3RigidBody::upcast(colObj0);
+	//	b3RigidBody* bodyB = b3RigidBody::upcast(colObj1);
 
 	b3SolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
 	b3SolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
 
-
-
 	///avoid collision response between two static objects
 	if (solverBodyA->m_invMass.isZero() && solverBodyB->m_invMass.isZero())
 		return;
 
-	int rollingFriction=1;
+	int rollingFriction = 1;
 	int numContacts = getNumContacts(manifold);
-	for (int j=0;j<numContacts;j++)
+	for (int j = 0; j < numContacts; j++)
 	{
-
 		b3ContactPoint cp;
-		getContactPoint(manifold,j,cp);
+		getContactPoint(manifold, j, cp);
 
 		if (cp.getDistance() <= getContactProcessingThreshold(manifold))
 		{
@@ -953,61 +878,60 @@ void	b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* i
 
 			int frictionIndex = m_tmpSolverContactConstraintPool.size();
 			b3SolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
-//			b3RigidBody* rb0 = b3RigidBody::upcast(colObj0);
-//			b3RigidBody* rb1 = b3RigidBody::upcast(colObj1);
+			//			b3RigidBody* rb0 = b3RigidBody::upcast(colObj0);
+			//			b3RigidBody* rb1 = b3RigidBody::upcast(colObj1);
 			solverConstraint.m_solverBodyIdA = solverBodyIdA;
 			solverConstraint.m_solverBodyIdB = solverBodyIdB;
 
 			solverConstraint.m_originalContactPoint = &cp;
 
-			setupContactConstraint(bodies,inertias,solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
+			setupContactConstraint(bodies, inertias, solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
 
-//			const b3Vector3& pos1 = cp.getPositionWorldOnA();
-//			const b3Vector3& pos2 = cp.getPositionWorldOnB();
+			//			const b3Vector3& pos1 = cp.getPositionWorldOnA();
+			//			const b3Vector3& pos2 = cp.getPositionWorldOnB();
 
 			/////setup the friction constraints
 
 			solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
 
-			b3Vector3 angVelA,angVelB;
+			b3Vector3 angVelA, angVelB;
 			solverBodyA->getAngularVelocity(angVelA);
-			solverBodyB->getAngularVelocity(angVelB);			
-			b3Vector3 relAngVel = angVelB-angVelA;
+			solverBodyB->getAngularVelocity(angVelB);
+			b3Vector3 relAngVel = angVelB - angVelA;
 
-			if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
+			if ((cp.m_combinedRollingFriction > 0.f) && (rollingFriction > 0))
 			{
 				//only a single rollingFriction per manifold
 				rollingFriction--;
-				if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
+				if (relAngVel.length() > infoGlobal.m_singleAxisRollingFrictionThreshold)
 				{
 					relAngVel.normalize();
-					if (relAngVel.length()>0.001)
-						addRollingFrictionConstraint(bodies,inertias,relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
-				} else
+					if (relAngVel.length() > 0.001)
+						addRollingFrictionConstraint(bodies, inertias, relAngVel, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+				}
+				else
 				{
-					addRollingFrictionConstraint(bodies,inertias,cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-					b3Vector3 axis0,axis1;
-					b3PlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
-					if (axis0.length()>0.001)
-						addRollingFrictionConstraint(bodies,inertias,axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-					if (axis1.length()>0.001)
-						addRollingFrictionConstraint(bodies,inertias,axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-		
+					addRollingFrictionConstraint(bodies, inertias, cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+					b3Vector3 axis0, axis1;
+					b3PlaneSpace1(cp.m_normalWorldOnB, axis0, axis1);
+					if (axis0.length() > 0.001)
+						addRollingFrictionConstraint(bodies, inertias, axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+					if (axis1.length() > 0.001)
+						addRollingFrictionConstraint(bodies, inertias, axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
 				}
 			}
 
 			///Bullet has several options to set the friction directions
-			///By default, each contact has only a single friction direction that is recomputed automatically very frame 
+			///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 B3_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 B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
 			///
-			///The user can manually override the friction directions for certain contacts using a contact callback, 
+			///The user can manually override the friction directions for certain contacts using a contact callback,
 			///and set the cp.m_lateralFrictionInitialized to true
 			///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
 			///this will give a conveyor belt effect
@@ -1018,99 +942,91 @@ void	b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* i
 				b3Scalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
 				if (!(infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > B3_EPSILON)
 				{
-					cp.m_lateralFrictionDir1 *= 1.f/b3Sqrt(lat_rel_vel);
-					if((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
+					cp.m_lateralFrictionDir1 *= 1.f / b3Sqrt(lat_rel_vel);
+					if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
 						cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
-						cp.m_lateralFrictionDir2.normalize();//??
-						addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
+						cp.m_lateralFrictionDir2.normalize();  //??
+						addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
 					}
 
-					addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
-				} else
+					addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+				}
+				else
 				{
-					b3PlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+					b3PlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2);
 
 					if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
-						addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+						addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
 					}
 
-					addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+					addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
 
 					if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
 					{
 						cp.m_lateralFrictionInitialized = true;
 					}
 				}
-
-			} else
+			}
+			else
 			{
-				addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
+				addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion1, cp.m_contactCFM1);
 
 				if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
-					addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
+					addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
 
-				setFrictionConstraintImpulse( bodies,inertias,solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
+				setFrictionConstraintImpulse(bodies, inertias, solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
 			}
-		
-
-			
-
 		}
 	}
 }
 
-b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal)
+b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal)
 {
 	B3_PROFILE("solveGroupCacheFriendlySetup");
 
-
 	m_maxOverrideNumSolverIterations = 0;
 
-
-
 	m_tmpSolverBodyPool.resize(0);
-	
-	
+
 	m_bodyCount.resize(0);
-	m_bodyCount.resize(numBodies,0);
+	m_bodyCount.resize(numBodies, 0);
 	m_bodyCountCheck.resize(0);
-	m_bodyCountCheck.resize(numBodies,0);
-	
+	m_bodyCountCheck.resize(numBodies, 0);
+
 	m_deltaLinearVelocities.resize(0);
-	m_deltaLinearVelocities.resize(numBodies,b3MakeVector3(0,0,0));
+	m_deltaLinearVelocities.resize(numBodies, b3MakeVector3(0, 0, 0));
 	m_deltaAngularVelocities.resize(0);
-	m_deltaAngularVelocities.resize(numBodies,b3MakeVector3(0,0,0));
-	
+	m_deltaAngularVelocities.resize(numBodies, b3MakeVector3(0, 0, 0));
+
 	//int totalBodies = 0;
 
-	for (int i=0;i<numConstraints;i++)
+	for (int i = 0; i < numConstraints; i++)
 	{
 		int bodyIndexA = constraints[i]->getRigidBodyA();
 		int bodyIndexB = constraints[i]->getRigidBodyB();
 		if (m_usePgs)
 		{
-			m_bodyCount[bodyIndexA]=-1;
-			m_bodyCount[bodyIndexB]=-1;
-		} else
+			m_bodyCount[bodyIndexA] = -1;
+			m_bodyCount[bodyIndexB] = -1;
+		}
+		else
 		{
 			//didn't implement joints with Jacobi version yet
 			b3Assert(0);
 		}
-
 	}
-	for (int i=0;i<numManifolds;i++)
+	for (int i = 0; i < numManifolds; i++)
 	{
 		int bodyIndexA = manifoldPtr[i].getBodyA();
 		int bodyIndexB = manifoldPtr[i].getBodyB();
 		if (m_usePgs)
 		{
-			m_bodyCount[bodyIndexA]=-1;
-			m_bodyCount[bodyIndexB]=-1;
-		} else
+			m_bodyCount[bodyIndexA] = -1;
+			m_bodyCount[bodyIndexB] = -1;
+		}
+		else
 		{
 			if (bodies[bodyIndexA].m_invMass)
 			{
@@ -1118,26 +1034,23 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
 				m_bodyCount[bodyIndexA]++;
 			}
 			else
-				m_bodyCount[bodyIndexA]=-1;
+				m_bodyCount[bodyIndexA] = -1;
 
 			if (bodies[bodyIndexB].m_invMass)
-			//	m_bodyCount[bodyIndexB]+=manifoldPtr[i].getNPoints();
+				//	m_bodyCount[bodyIndexB]+=manifoldPtr[i].getNPoints();
 				m_bodyCount[bodyIndexB]++;
 			else
-				m_bodyCount[bodyIndexB]=-1;
+				m_bodyCount[bodyIndexB] = -1;
 		}
-
 	}
 
-
-	
 	if (1)
 	{
 		int j;
-		for (j=0;j<numConstraints;j++)
+		for (j = 0; j < numConstraints; j++)
 		{
 			b3TypedConstraint* constraint = constraints[j];
-			
+
 			constraint->internalSetAppliedImpulse(0.0f);
 		}
 	}
@@ -1146,13 +1059,12 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
 	//if (1)
 	{
 		{
-
 			int totalNumRows = 0;
 			int i;
-			
+
 			m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
 			//calculate the total number of contraint rows
-			for (i=0;i<numConstraints;i++)
+			for (i = 0; i < numConstraints; i++)
 			{
 				b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
 				b3JointFeedback* fb = constraints[i]->getJointFeedback();
@@ -1169,8 +1081,9 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
 				}
 				if (constraints[i]->isEnabled())
 				{
-					constraints[i]->getInfo1(&info1,bodies);
-				} else
+					constraints[i]->getInfo1(&info1, bodies);
+				}
+				else
 				{
 					info1.m_numConstraintRows = 0;
 					info1.nub = 0;
@@ -1179,45 +1092,40 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
 			}
 			m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
 
-			
 #ifndef DISABLE_JOINTS
 			///setup the b3SolverConstraints
 			int currentRow = 0;
 
-			for (i=0;i<numConstraints;i++)
+			for (i = 0; i < numConstraints; i++)
 			{
 				const b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
-				
+
 				if (info1.m_numConstraintRows)
 				{
-					b3Assert(currentRow<totalNumRows);
+					b3Assert(currentRow < totalNumRows);
 
 					b3SolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
 					b3TypedConstraint* constraint = constraints[i];
 
-					b3RigidBodyData& rbA = bodies[ constraint->getRigidBodyA()];
+					b3RigidBodyData& rbA = bodies[constraint->getRigidBodyA()];
 					//b3RigidBody& rbA = constraint->getRigidBodyA();
-	//				b3RigidBody& rbB = constraint->getRigidBodyB();
-					b3RigidBodyData& rbB = bodies[ constraint->getRigidBodyB()];
-
-                    int solverBodyIdA = getOrInitSolverBody(constraint->getRigidBodyA(),bodies,inertias);
-                    int solverBodyIdB = getOrInitSolverBody(constraint->getRigidBodyB(),bodies,inertias);
-
-                    b3SolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
-                    b3SolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
-
+					//				b3RigidBody& rbB = constraint->getRigidBodyB();
+					b3RigidBodyData& rbB = bodies[constraint->getRigidBodyB()];
 
+					int solverBodyIdA = getOrInitSolverBody(constraint->getRigidBodyA(), bodies, inertias);
+					int solverBodyIdB = getOrInitSolverBody(constraint->getRigidBodyB(), bodies, inertias);
 
+					b3SolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
+					b3SolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
 
 					int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
-					if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
+					if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations)
 						m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
 
-
 					int j;
-					for ( j=0;j<info1.m_numConstraintRows;j++)
+					for (j = 0; j < info1.m_numConstraintRows; j++)
 					{
-						memset(&currentConstraintRow[j],0,sizeof(b3SolverConstraint));
+						memset(&currentConstraintRow[j], 0, sizeof(b3SolverConstraint));
 						currentConstraintRow[j].m_lowerLimit = -B3_INFINITY;
 						currentConstraintRow[j].m_upperLimit = B3_INFINITY;
 						currentConstraintRow[j].m_appliedImpulse = 0.f;
@@ -1227,26 +1135,25 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
 						currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
 					}
 
-					bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
-					bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
-					bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
-					bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
-					bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
-					bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
-					bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
-					bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
-
+					bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f);
+					bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f);
+					bodyAPtr->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
+					bodyAPtr->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
+					bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f);
+					bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f);
+					bodyBPtr->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
+					bodyBPtr->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
 
 					b3TypedConstraint::b3ConstraintInfo2 info2;
-					info2.fps = 1.f/infoGlobal.m_timeStep;
+					info2.fps = 1.f / infoGlobal.m_timeStep;
 					info2.erp = infoGlobal.m_erp;
 					info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
 					info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
 					info2.m_J2linearAxis = 0;
 					info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
-					info2.rowskip = sizeof(b3SolverConstraint)/sizeof(b3Scalar);//check this
-					///the size of b3SolverConstraint needs be a multiple of b3Scalar
-		            b3Assert(info2.rowskip*sizeof(b3Scalar)== sizeof(b3SolverConstraint));
+					info2.rowskip = sizeof(b3SolverConstraint) / sizeof(b3Scalar);  //check this
+																					///the size of b3SolverConstraint needs be a multiple of b3Scalar
+					b3Assert(info2.rowskip * sizeof(b3Scalar) == sizeof(b3SolverConstraint));
 					info2.m_constraintError = &currentConstraintRow->m_rhs;
 					currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
 					info2.m_damping = infoGlobal.m_damping;
@@ -1254,47 +1161,45 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
 					info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
 					info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
 					info2.m_numIterations = infoGlobal.m_numIterations;
-					constraints[i]->getInfo2(&info2,bodies);
+					constraints[i]->getInfo2(&info2, bodies);
 
 					///finalize the constraint setup
-					for ( j=0;j<info1.m_numConstraintRows;j++)
+					for (j = 0; j < info1.m_numConstraintRows; j++)
 					{
 						b3SolverConstraint& solverConstraint = currentConstraintRow[j];
 
-						if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold())
+						if (solverConstraint.m_upperLimit >= constraints[i]->getBreakingImpulseThreshold())
 						{
 							solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
 						}
 
-						if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold())
+						if (solverConstraint.m_lowerLimit <= -constraints[i]->getBreakingImpulseThreshold())
 						{
 							solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
 						}
 
 						solverConstraint.m_originalContactPoint = constraint;
-							
-						b3Matrix3x3& invInertiaWorldA= inertias[constraint->getRigidBodyA()].m_invInertiaWorld;
-						{
 
+						b3Matrix3x3& invInertiaWorldA = inertias[constraint->getRigidBodyA()].m_invInertiaWorld;
+						{
 							//b3Vector3 angularFactorA(1,1,1);
 							const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
-							solverConstraint.m_angularComponentA = invInertiaWorldA*ftorqueAxis1;//*angularFactorA;
+							solverConstraint.m_angularComponentA = invInertiaWorldA * ftorqueAxis1;  //*angularFactorA;
 						}
-						
-						b3Matrix3x3& invInertiaWorldB= inertias[constraint->getRigidBodyB()].m_invInertiaWorld;
-						{
 
+						b3Matrix3x3& invInertiaWorldB = inertias[constraint->getRigidBodyB()].m_invInertiaWorld;
+						{
 							const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
-							solverConstraint.m_angularComponentB = invInertiaWorldB*ftorqueAxis2;//*constraint->getRigidBodyB().getAngularFactor();
+							solverConstraint.m_angularComponentB = invInertiaWorldB * ftorqueAxis2;  //*constraint->getRigidBodyB().getAngularFactor();
 						}
 
 						{
 							//it is ok to use solverConstraint.m_contactNormal instead of -solverConstraint.m_contactNormal
 							//because it gets multiplied iMJlB
-							b3Vector3 iMJlA = solverConstraint.m_contactNormal*rbA.m_invMass;
-							b3Vector3 iMJaA = invInertiaWorldA*solverConstraint.m_relpos1CrossNormal;
-							b3Vector3 iMJlB = solverConstraint.m_contactNormal*rbB.m_invMass;//sign of normal?
-							b3Vector3 iMJaB = invInertiaWorldB*solverConstraint.m_relpos2CrossNormal;
+							b3Vector3 iMJlA = solverConstraint.m_contactNormal * rbA.m_invMass;
+							b3Vector3 iMJaA = invInertiaWorldA * solverConstraint.m_relpos1CrossNormal;
+							b3Vector3 iMJlB = solverConstraint.m_contactNormal * rbB.m_invMass;  //sign of normal?
+							b3Vector3 iMJaB = invInertiaWorldB * solverConstraint.m_relpos2CrossNormal;
 
 							b3Scalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
 							sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
@@ -1302,10 +1207,9 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
 							sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
 							b3Scalar fsum = b3Fabs(sum);
 							b3Assert(fsum > B3_EPSILON);
-							solverConstraint.m_jacDiagABInv = fsum>B3_EPSILON?b3Scalar(1.)/sum : 0.f;
+							solverConstraint.m_jacDiagABInv = fsum > B3_EPSILON ? b3Scalar(1.) / sum : 0.f;
 						}
 
-
 						///fix rhs
 						///todo: add force/torque accelerators
 						{
@@ -1313,38 +1217,35 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
 							b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.m_linVel) + solverConstraint.m_relpos1CrossNormal.dot(rbA.m_angVel);
 							b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.m_linVel) + solverConstraint.m_relpos2CrossNormal.dot(rbB.m_angVel);
 
-							rel_vel = vel1Dotn+vel2Dotn;
+							rel_vel = vel1Dotn + vel2Dotn;
 
 							b3Scalar restitution = 0.f;
-							b3Scalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
-							b3Scalar	velocityError = restitution - rel_vel * info2.m_damping;
-							b3Scalar	penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
-							b3Scalar	velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
-							solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+							b3Scalar positionalError = solverConstraint.m_rhs;  //already filled in by getConstraintInfo2
+							b3Scalar velocityError = restitution - rel_vel * info2.m_damping;
+							b3Scalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
+							b3Scalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
+							solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
 							solverConstraint.m_appliedImpulse = 0.f;
-
 						}
 					}
 				}
-				currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
+				currentRow += m_tmpConstraintSizesPool[i].m_numConstraintRows;
 			}
-#endif //DISABLE_JOINTS
+#endif  //DISABLE_JOINTS
 		}
 
-
 		{
 			int i;
 
-			for (i=0;i<numManifolds;i++)
+			for (i = 0; i < numManifolds; i++)
 			{
 				b3Contact4& manifold = manifoldPtr[i];
-				convertContact(bodies,inertias,&manifold,infoGlobal);
+				convertContact(bodies, inertias, &manifold, infoGlobal);
 			}
 		}
 	}
 
-//	b3ContactSolverInfo info = infoGlobal;
-
+	//	b3ContactSolverInfo info = infoGlobal;
 
 	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
 	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
@@ -1353,64 +1254,63 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
 	///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
 	m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
 	if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
-		m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2);
+		m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2);
 	else
 		m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
 
 	m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
 	{
 		int i;
-		for (i=0;i<numNonContactPool;i++)
+		for (i = 0; i < numNonContactPool; i++)
 		{
 			m_orderNonContactConstraintPool[i] = i;
 		}
-		for (i=0;i<numConstraintPool;i++)
+		for (i = 0; i < numConstraintPool; i++)
 		{
 			m_orderTmpConstraintPool[i] = i;
 		}
-		for (i=0;i<numFrictionPool;i++)
+		for (i = 0; i < numFrictionPool; i++)
 		{
 			m_orderFrictionConstraintPool[i] = i;
 		}
 	}
 
 	return 0.f;
-
 }
 
-
-b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal)
+b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal)
 {
-
 	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
 	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
 	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
-	
+
 	if (infoGlobal.m_solverMode & B3_SOLVER_RANDMIZE_ORDER)
 	{
-		if (1)			// uncomment this for a bit less random ((iteration & 7) == 0)
+		if (1)  // uncomment this for a bit less random ((iteration & 7) == 0)
 		{
-
-			for (int j=0; j<numNonContactPool; ++j) {
+			for (int j = 0; j < numNonContactPool; ++j)
+			{
 				int tmp = m_orderNonContactConstraintPool[j];
-				int swapi = b3RandInt2(j+1);
+				int swapi = b3RandInt2(j + 1);
 				m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
 				m_orderNonContactConstraintPool[swapi] = tmp;
 			}
 
-			//contact/friction constraints are not solved more than 
-			if (iteration< infoGlobal.m_numIterations)
+			//contact/friction constraints are not solved more than
+			if (iteration < infoGlobal.m_numIterations)
 			{
-				for (int j=0; j<numConstraintPool; ++j) {
+				for (int j = 0; j < numConstraintPool; ++j)
+				{
 					int tmp = m_orderTmpConstraintPool[j];
-					int swapi = b3RandInt2(j+1);
+					int swapi = b3RandInt2(j + 1);
 					m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
 					m_orderTmpConstraintPool[swapi] = tmp;
 				}
 
-				for (int j=0; j<numFrictionPool; ++j) {
+				for (int j = 0; j < numFrictionPool; ++j)
+				{
 					int tmp = m_orderFrictionConstraintPool[j];
-					int swapi = b3RandInt2(j+1);
+					int swapi = b3RandInt2(j + 1);
 					m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
 					m_orderFrictionConstraintPool[swapi] = tmp;
 				}
@@ -1421,173 +1321,163 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
 	if (infoGlobal.m_solverMode & B3_SOLVER_SIMD)
 	{
 		///solve all joint constraints, using SIMD, if available
-		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
 		{
 			b3SolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
 			if (iteration < constraint.m_overrideNumSolverIterations)
-				resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint);
 		}
 
-		if (iteration< infoGlobal.m_numIterations)
+		if (iteration < infoGlobal.m_numIterations)
 		{
-
 			///solve all contact constraints using SIMD, if available
 			if (infoGlobal.m_solverMode & B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
 			{
 				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-				int multiplier = (infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
+				int multiplier = (infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
 
-				for (int c=0;c<numPoolConstraints;c++)
+				for (int c = 0; c < numPoolConstraints; c++)
 				{
-					b3Scalar totalImpulse =0;
+					b3Scalar totalImpulse = 0;
 
 					{
 						const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
-						resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 						totalImpulse = solveManifold.m_appliedImpulse;
 					}
 					bool applyFriction = true;
 					if (applyFriction)
 					{
 						{
+							b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]];
 
-							b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
-
-							if (totalImpulse>b3Scalar(0))
+							if (totalImpulse > b3Scalar(0))
 							{
-								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
 
-								resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 							}
 						}
 
 						if (infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)
 						{
+							b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]];
 
-							b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
-				
-							if (totalImpulse>b3Scalar(0))
+							if (totalImpulse > b3Scalar(0))
 							{
-								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
 
-								resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 							}
 						}
 					}
 				}
-
 			}
-			else//B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
+			else  //B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
 			{
 				//solve the friction constraints after all contact constraints, don't interleave them
 				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
 				int j;
 
-				for (j=0;j<numPoolConstraints;j++)
+				for (j = 0; j < numPoolConstraints; j++)
 				{
 					const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-					resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-
+					resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 				}
 
 				if (!m_usePgs)
 					averageVelocities();
-				
 
 				///solve all friction constraints, using SIMD, if available
 
 				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-				for (j=0;j<numFrictionPoolConstraints;j++)
+				for (j = 0; j < numFrictionPoolConstraints; j++)
 				{
 					b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
 					b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
 
-					if (totalImpulse>b3Scalar(0))
+					if (totalImpulse > b3Scalar(0))
 					{
-						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+						solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+						solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
 
-						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 					}
 				}
 
-				
 				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-				for (j=0;j<numRollingFrictionPoolConstraints;j++)
+				for (j = 0; j < numRollingFrictionPoolConstraints; j++)
 				{
-
 					b3SolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
 					b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
-					if (totalImpulse>b3Scalar(0))
+					if (totalImpulse > b3Scalar(0))
 					{
-						b3Scalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
-						if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
+						b3Scalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
+						if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
 							rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
 
 						rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
 						rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
 
-						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
 					}
 				}
-				
-
-			}			
+			}
 		}
-	} else
+	}
+	else
 	{
 		//non-SIMD version
 		///solve all joint constraints
-		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
 		{
 			b3SolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
 			if (iteration < constraint.m_overrideNumSolverIterations)
-				resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint);
 		}
 
-		if (iteration< infoGlobal.m_numIterations)
+		if (iteration < infoGlobal.m_numIterations)
 		{
-
 			///solve all contact constraints
 			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-			for (int j=0;j<numPoolConstraints;j++)
+			for (int j = 0; j < numPoolConstraints; j++)
 			{
 				const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-				resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+				resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 			}
 			///solve all friction constraints
 			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-			for (int j=0;j<numFrictionPoolConstraints;j++)
+			for (int j = 0; j < numFrictionPoolConstraints; j++)
 			{
 				b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
 				b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
 
-				if (totalImpulse>b3Scalar(0))
+				if (totalImpulse > b3Scalar(0))
 				{
-					solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-					solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+					solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
+					solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
 
-					resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 				}
 			}
 
 			int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
-			for (int j=0;j<numRollingFrictionPoolConstraints;j++)
+			for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
 			{
 				b3SolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
 				b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
-				if (totalImpulse>b3Scalar(0))
+				if (totalImpulse > b3Scalar(0))
 				{
-					b3Scalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
-					if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
+					b3Scalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
+					if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
 						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
 
 					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
 					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
 
-					resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+					resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
 				}
 			}
 		}
@@ -1595,40 +1485,39 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
 	return 0.f;
 }
 
-
-void b3PgsJacobiSolver::solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal)
+void b3PgsJacobiSolver::solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal)
 {
 	int iteration;
 	if (infoGlobal.m_splitImpulse)
 	{
 		if (infoGlobal.m_solverMode & B3_SOLVER_SIMD)
 		{
-			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+			for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
 			{
 				{
 					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
 					int j;
-					for (j=0;j<numPoolConstraints;j++)
+					for (j = 0; j < numPoolConstraints; j++)
 					{
 						const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
 
-						resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 					}
 				}
 			}
 		}
 		else
 		{
-			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+			for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
 			{
 				{
 					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
 					int j;
-					for (j=0;j<numPoolConstraints;j++)
+					for (j = 0; j < numPoolConstraints; j++)
 					{
 						const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
 
-						resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 					}
 				}
 			}
@@ -1636,100 +1525,95 @@ void b3PgsJacobiSolver::solveGroupCacheFriendlySplitImpulseIterations(b3TypedCon
 	}
 }
 
-b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal)
+b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal)
 {
 	B3_PROFILE("solveGroupCacheFriendlyIterations");
 
 	{
 		///this is a special step to resolve penetrations (just for contacts)
-		solveGroupCacheFriendlySplitImpulseIterations(constraints,numConstraints,infoGlobal);
+		solveGroupCacheFriendlySplitImpulseIterations(constraints, numConstraints, infoGlobal);
 
-		int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
+		int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
 
-		for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
+		for (int iteration = 0; iteration < maxIterations; iteration++)
 		//for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
-		{			
-			
-			solveSingleIteration(iteration, constraints,numConstraints,infoGlobal);
-
+		{
+			solveSingleIteration(iteration, constraints, numConstraints, infoGlobal);
 
 			if (!m_usePgs)
 			{
 				averageVelocities();
 			}
 		}
-		
 	}
 	return 0.f;
 }
 
-void	b3PgsJacobiSolver::averageVelocities()
+void b3PgsJacobiSolver::averageVelocities()
 {
 	B3_PROFILE("averaging");
 	//average the velocities
 	int numBodies = m_bodyCount.size();
 
 	m_deltaLinearVelocities.resize(0);
-	m_deltaLinearVelocities.resize(numBodies,b3MakeVector3(0,0,0));
+	m_deltaLinearVelocities.resize(numBodies, b3MakeVector3(0, 0, 0));
 	m_deltaAngularVelocities.resize(0);
-	m_deltaAngularVelocities.resize(numBodies,b3MakeVector3(0,0,0));
+	m_deltaAngularVelocities.resize(numBodies, b3MakeVector3(0, 0, 0));
 
-	for (int i=0;i<m_tmpSolverBodyPool.size();i++)
+	for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
 	{
 		if (!m_tmpSolverBodyPool[i].m_invMass.isZero())
 		{
 			int orgBodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex;
-			m_deltaLinearVelocities[orgBodyIndex]+=m_tmpSolverBodyPool[i].getDeltaLinearVelocity();
-			m_deltaAngularVelocities[orgBodyIndex]+=m_tmpSolverBodyPool[i].getDeltaAngularVelocity();
+			m_deltaLinearVelocities[orgBodyIndex] += m_tmpSolverBodyPool[i].getDeltaLinearVelocity();
+			m_deltaAngularVelocities[orgBodyIndex] += m_tmpSolverBodyPool[i].getDeltaAngularVelocity();
 		}
 	}
-				
-	for (int i=0;i<m_tmpSolverBodyPool.size();i++)
+
+	for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
 	{
 		int orgBodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex;
 
 		if (!m_tmpSolverBodyPool[i].m_invMass.isZero())
 		{
-						
 			b3Assert(m_bodyCount[orgBodyIndex] == m_bodyCountCheck[orgBodyIndex]);
-						
-			b3Scalar factor = 1.f/b3Scalar(m_bodyCount[orgBodyIndex]);
-						
 
-			m_tmpSolverBodyPool[i].m_deltaLinearVelocity = m_deltaLinearVelocities[orgBodyIndex]*factor;
-			m_tmpSolverBodyPool[i].m_deltaAngularVelocity = m_deltaAngularVelocities[orgBodyIndex]*factor;
+			b3Scalar factor = 1.f / b3Scalar(m_bodyCount[orgBodyIndex]);
+
+			m_tmpSolverBodyPool[i].m_deltaLinearVelocity = m_deltaLinearVelocities[orgBodyIndex] * factor;
+			m_tmpSolverBodyPool[i].m_deltaAngularVelocity = m_deltaAngularVelocities[orgBodyIndex] * factor;
 		}
 	}
 }
 
-b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal)
+b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, const b3ContactSolverInfo& infoGlobal)
 {
 	B3_PROFILE("solveGroupCacheFriendlyFinish");
 	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-	int i,j;
+	int i, j;
 
 	if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING)
 	{
-		for (j=0;j<numPoolConstraints;j++)
+		for (j = 0; j < numPoolConstraints; j++)
 		{
 			const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
-			b3ContactPoint* pt = (b3ContactPoint*) solveManifold.m_originalContactPoint;
+			b3ContactPoint* pt = (b3ContactPoint*)solveManifold.m_originalContactPoint;
 			b3Assert(pt);
 			pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
-		//	float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+			//	float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
 			//	printf("pt->m_appliedImpulseLateral1 = %f\n", f);
 			pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
 			//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
 			if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
 			{
-				pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
+				pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse;
 			}
 			//do a callback here?
 		}
 	}
 
 	numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
-	for (j=0;j<numPoolConstraints;j++)
+	for (j = 0; j < numPoolConstraints; j++)
 	{
 		const b3SolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
 		b3TypedConstraint* constr = (b3TypedConstraint*)solverConstr.m_originalContactPoint;
@@ -1739,15 +1623,14 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodie
 			b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverConstr.m_solverBodyIdA];
 			b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverConstr.m_solverBodyIdB];
 
-			fb->m_appliedForceBodyA += solverConstr.m_contactNormal*solverConstr.m_appliedImpulse*bodyA->m_linearFactor/infoGlobal.m_timeStep;
-			fb->m_appliedForceBodyB += -solverConstr.m_contactNormal*solverConstr.m_appliedImpulse*bodyB->m_linearFactor/infoGlobal.m_timeStep;
-			fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* bodyA->m_angularFactor*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
-			fb->m_appliedTorqueBodyB += -solverConstr.m_relpos1CrossNormal* bodyB->m_angularFactor*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
-			
+			fb->m_appliedForceBodyA += solverConstr.m_contactNormal * solverConstr.m_appliedImpulse * bodyA->m_linearFactor / infoGlobal.m_timeStep;
+			fb->m_appliedForceBodyB += -solverConstr.m_contactNormal * solverConstr.m_appliedImpulse * bodyB->m_linearFactor / infoGlobal.m_timeStep;
+			fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal * bodyA->m_angularFactor * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep;
+			fb->m_appliedTorqueBodyB += -solverConstr.m_relpos1CrossNormal * bodyB->m_angularFactor * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep;
 		}
 
 		constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
-		if (b3Fabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
+		if (b3Fabs(solverConstr.m_appliedImpulse) >= constr->getBreakingImpulseThreshold())
 		{
 			constr->setEnabled(false);
 		}
@@ -1755,7 +1638,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodie
 
 	{
 		B3_PROFILE("write back velocities and transforms");
-		for ( i=0;i<m_tmpSolverBodyPool.size();i++)
+		for (i = 0; i < m_tmpSolverBodyPool.size(); i++)
 		{
 			int bodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex;
 			//b3Assert(i==bodyIndex);
@@ -1772,12 +1655,13 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodie
 				{
 					body->m_linVel = m_tmpSolverBodyPool[i].m_linearVelocity;
 					body->m_angVel = m_tmpSolverBodyPool[i].m_angularVelocity;
-				} else
+				}
+				else
 				{
-					b3Scalar factor = 1.f/b3Scalar(m_bodyCount[bodyIndex]);
+					b3Scalar factor = 1.f / b3Scalar(m_bodyCount[bodyIndex]);
 
-					b3Vector3 deltaLinVel = m_deltaLinearVelocities[bodyIndex]*factor;
-					b3Vector3 deltaAngVel = m_deltaAngularVelocities[bodyIndex]*factor;
+					b3Vector3 deltaLinVel = m_deltaLinearVelocities[bodyIndex] * factor;
+					b3Vector3 deltaAngVel = m_deltaAngularVelocities[bodyIndex] * factor;
 					//printf("body %d\n",bodyIndex);
 					//printf("deltaLinVel = %f,%f,%f\n",deltaLinVel.getX(),deltaLinVel.getY(),deltaLinVel.getZ());
 					//printf("deltaAngVel = %f,%f,%f\n",deltaAngVel.getX(),deltaAngVel.getY(),deltaAngVel.getZ());
@@ -1785,7 +1669,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodie
 					body->m_linVel += deltaLinVel;
 					body->m_angVel += deltaAngVel;
 				}
-			
+
 				if (infoGlobal.m_splitImpulse)
 				{
 					body->m_pos = m_tmpSolverBodyPool[i].m_worldTransform.getOrigin();
@@ -1797,7 +1681,6 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodie
 		}
 	}
 
-
 	m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
 	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
 	m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
@@ -1807,9 +1690,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodie
 	return 0.f;
 }
 
-
-
-void	b3PgsJacobiSolver::reset()
+void b3PgsJacobiSolver::reset()
 {
 	m_btSeed2 = 0;
 }
\ No newline at end of file
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h
index d2ca307fab..5b616541d9 100644
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h
@@ -1,11 +1,9 @@
 #ifndef B3_PGS_JACOBI_SOLVER
 #define B3_PGS_JACOBI_SOLVER
 
-
 struct b3Contact4;
 struct b3ContactPoint;
 
-
 class b3Dispatcher;
 
 #include "b3TypedConstraint.h"
@@ -18,132 +16,118 @@ struct b3InertiaData;
 
 class b3PgsJacobiSolver
 {
-
 protected:
-	b3AlignedObjectArray<b3SolverBody>      m_tmpSolverBodyPool;
-	b3ConstraintArray			m_tmpSolverContactConstraintPool;
-	b3ConstraintArray			m_tmpSolverNonContactConstraintPool;
-	b3ConstraintArray			m_tmpSolverContactFrictionConstraintPool;
-	b3ConstraintArray			m_tmpSolverContactRollingFrictionConstraintPool;
-
-	b3AlignedObjectArray<int>	m_orderTmpConstraintPool;
-	b3AlignedObjectArray<int>	m_orderNonContactConstraintPool;
-	b3AlignedObjectArray<int>	m_orderFrictionConstraintPool;
+	b3AlignedObjectArray<b3SolverBody> m_tmpSolverBodyPool;
+	b3ConstraintArray m_tmpSolverContactConstraintPool;
+	b3ConstraintArray m_tmpSolverNonContactConstraintPool;
+	b3ConstraintArray m_tmpSolverContactFrictionConstraintPool;
+	b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
+
+	b3AlignedObjectArray<int> m_orderTmpConstraintPool;
+	b3AlignedObjectArray<int> m_orderNonContactConstraintPool;
+	b3AlignedObjectArray<int> m_orderFrictionConstraintPool;
 	b3AlignedObjectArray<b3TypedConstraint::b3ConstraintInfo1> m_tmpConstraintSizesPool;
-	
-	b3AlignedObjectArray<int>		m_bodyCount;
-	b3AlignedObjectArray<int>		m_bodyCountCheck;
-	
-	b3AlignedObjectArray<b3Vector3>	m_deltaLinearVelocities;
-	b3AlignedObjectArray<b3Vector3>	m_deltaAngularVelocities;
 
-	bool						m_usePgs;
-	void						averageVelocities();
+	b3AlignedObjectArray<int> m_bodyCount;
+	b3AlignedObjectArray<int> m_bodyCountCheck;
+
+	b3AlignedObjectArray<b3Vector3> m_deltaLinearVelocities;
+	b3AlignedObjectArray<b3Vector3> m_deltaAngularVelocities;
+
+	bool m_usePgs;
+	void averageVelocities();
 
-	int							m_maxOverrideNumSolverIterations;
+	int m_maxOverrideNumSolverIterations;
 
-	int							m_numSplitImpulseRecoveries;
+	int m_numSplitImpulseRecoveries;
 
-	b3Scalar	getContactProcessingThreshold(b3Contact4* contact)
+	b3Scalar getContactProcessingThreshold(b3Contact4* contact)
 	{
 		return 0.02f;
 	}
-	void setupFrictionConstraint(	b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
-									b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
-									b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, 
-									b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
+	void setupFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
+								 b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
+								 b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
+								 b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
 
-	void setupRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,	b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
-									b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
-									b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, 
-									b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
-
-	b3SolverConstraint&	addFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
-	b3SolverConstraint&	addRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0, b3Scalar cfmSlip=0.f);
+	void setupRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
+										b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
+										b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
+										b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
 
+	b3SolverConstraint& addFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
+	b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0, b3Scalar cfmSlip = 0.f);
 
 	void setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias,
-								b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp, 
-								const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation, 
+								b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp,
+								const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
 								b3Vector3& rel_pos1, b3Vector3& rel_pos2);
 
-	void setFrictionConstraintImpulse( b3RigidBodyData* bodies, b3InertiaData* inertias,b3SolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, 
-										 b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal);
+	void setFrictionConstraintImpulse(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB,
+									  b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal);
 
 	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
-	unsigned long	m_btSeed2;
+	unsigned long m_btSeed2;
 
-	
 	b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution);
 
-	void	convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal);
-
+	void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias, b3Contact4* manifold, const b3ContactSolverInfo& infoGlobal);
 
-	void	resolveSplitPenetrationSIMD(
-     b3SolverBody& bodyA,b3SolverBody& bodyB,
-        const b3SolverConstraint& contactConstraint);
+	void resolveSplitPenetrationSIMD(
+		b3SolverBody& bodyA, b3SolverBody& bodyB,
+		const b3SolverConstraint& contactConstraint);
 
-	void	resolveSplitPenetrationImpulseCacheFriendly(
-       b3SolverBody& bodyA,b3SolverBody& bodyB,
-        const b3SolverConstraint& contactConstraint);
+	void resolveSplitPenetrationImpulseCacheFriendly(
+		b3SolverBody& bodyA, b3SolverBody& bodyB,
+		const b3SolverConstraint& contactConstraint);
 
 	//internal method
-	int		getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies,b3InertiaData* inertias);
-	void	initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject);
+	int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies, b3InertiaData* inertias);
+	void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject);
 
-	void	resolveSingleConstraintRowGeneric(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
-
-	void	resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
-	
-	void	resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
-	
-	void	resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
-		
-protected:
+	void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
 
-	virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
+	void resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
 
+	void resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
 
-	virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
-	virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
-	b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
+	void resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
 
+protected:
+	virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
 
-	virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal);
+	virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
+	virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
+	b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
 
+	virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, const b3ContactSolverInfo& infoGlobal);
 
 public:
-
 	B3_DECLARE_ALIGNED_ALLOCATOR();
-	
+
 	b3PgsJacobiSolver(bool usePgs);
 	virtual ~b3PgsJacobiSolver();
 
-//	void	solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts);
-	void	solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);
+	//	void	solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts);
+	void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);
 
-	b3Scalar solveGroup(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
+	b3Scalar solveGroup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
 
 	///clear internal cached data and reset random seed
-	virtual	void	reset();
-	
+	virtual void reset();
+
 	unsigned long b3Rand2();
 
-	int b3RandInt2 (int n);
+	int b3RandInt2(int n);
 
-	void	setRandSeed(unsigned long seed)
+	void setRandSeed(unsigned long seed)
 	{
 		m_btSeed2 = seed;
 	}
-	unsigned long	getRandSeed() const
+	unsigned long getRandSeed() const
 	{
 		return m_btSeed2;
 	}
-
-
-
-
 };
 
-#endif //B3_PGS_JACOBI_SOLVER
-
+#endif  //B3_PGS_JACOBI_SOLVER
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp
index 02c11db320..cfa7c7dd11 100644
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp
@@ -13,21 +13,14 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "b3Point2PointConstraint.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
 
 #include <new>
 
-
-
-
-
-b3Point2PointConstraint::b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB)
-:b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
-m_flags(0)
+b3Point2PointConstraint::b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB)
+	: b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0)
 {
-
 }
 
 /*
@@ -40,22 +33,18 @@ m_useSolveConstraintObsolete(false)
 }
 */
 
-
-void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
+void b3Point2PointConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
 {
-	getInfo1NonVirtual(info,bodies);
+	getInfo1NonVirtual(info, bodies);
 }
 
-void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
+void b3Point2PointConstraint::getInfo1NonVirtual(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
 {
-		info->m_numConstraintRows = 3;
-		info->nub = 3;
+	info->m_numConstraintRows = 3;
+	info->nub = 3;
 }
 
-
-
-
-void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
+void b3Point2PointConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
 {
 	b3Transform trA;
 	trA.setIdentity();
@@ -67,143 +56,135 @@ void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBo
 	trB.setOrigin(bodies[m_rbB].m_pos);
 	trB.setRotation(bodies[m_rbB].m_quat);
 
-	getInfo2NonVirtual(info, trA,trB);
+	getInfo2NonVirtual(info, trA, trB);
 }
 
-void b3Point2PointConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
+void b3Point2PointConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
 {
-
-	 //retrieve matrices
+	//retrieve matrices
 
 	// anchor points in global coordinates with respect to body PORs.
-   
-    // set jacobian
-    info->m_J1linearAxis[0] = 1;
-	info->m_J1linearAxis[info->rowskip+1] = 1;
-	info->m_J1linearAxis[2*info->rowskip+2] = 1;
 
-	b3Vector3 a1 = body0_trans.getBasis()*getPivotInA();
+	// set jacobian
+	info->m_J1linearAxis[0] = 1;
+	info->m_J1linearAxis[info->rowskip + 1] = 1;
+	info->m_J1linearAxis[2 * info->rowskip + 2] = 1;
+
+	b3Vector3 a1 = body0_trans.getBasis() * getPivotInA();
 	//b3Vector3 a1a = b3QuatRotate(body0_trans.getRotation(),getPivotInA());
 
 	{
 		b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis);
-		b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip);
-		b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip);
+		b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip);
+		b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip);
 		b3Vector3 a1neg = -a1;
-		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
-    
+
 	if (info->m_J2linearAxis)
 	{
 		info->m_J2linearAxis[0] = -1;
-		info->m_J2linearAxis[info->rowskip+1] = -1;
-		info->m_J2linearAxis[2*info->rowskip+2] = -1;
+		info->m_J2linearAxis[info->rowskip + 1] = -1;
+		info->m_J2linearAxis[2 * info->rowskip + 2] = -1;
 	}
-	
-	b3Vector3 a2 = body1_trans.getBasis()*getPivotInB();
-   
+
+	b3Vector3 a2 = body1_trans.getBasis() * getPivotInB();
+
 	{
-	//	b3Vector3 a2n = -a2;
+		//	b3Vector3 a2n = -a2;
 		b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis);
-		b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip);
-		b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip);
-		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+		b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip);
+		b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
 	}
-    
 
-
-    // set right hand side
+	// set right hand side
 	b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp;
-    b3Scalar k = info->fps * currERP;
-    int j;
-	for (j=0; j<3; j++)
-    {
-        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
+	b3Scalar k = info->fps * currERP;
+	int j;
+	for (j = 0; j < 3; j++)
+	{
+		info->m_constraintError[j * info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
 		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
-    }
-	if(m_flags & B3_P2P_FLAGS_CFM)
+	}
+	if (m_flags & B3_P2P_FLAGS_CFM)
 	{
-		for (j=0; j<3; j++)
+		for (j = 0; j < 3; j++)
 		{
-			info->cfm[j*info->rowskip] = m_cfm;
+			info->cfm[j * info->rowskip] = m_cfm;
 		}
 	}
 
-	b3Scalar impulseClamp = m_setting.m_impulseClamp;//
-	for (j=0; j<3; j++)
-    {
+	b3Scalar impulseClamp = m_setting.m_impulseClamp;  //
+	for (j = 0; j < 3; j++)
+	{
 		if (m_setting.m_impulseClamp > 0)
 		{
-			info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
-			info->m_upperLimit[j*info->rowskip] = impulseClamp;
+			info->m_lowerLimit[j * info->rowskip] = -impulseClamp;
+			info->m_upperLimit[j * info->rowskip] = impulseClamp;
 		}
 	}
 	info->m_damping = m_setting.m_damping;
-	
 }
 
-
-
-void	b3Point2PointConstraint::updateRHS(b3Scalar	timeStep)
+void b3Point2PointConstraint::updateRHS(b3Scalar timeStep)
 {
 	(void)timeStep;
-
 }
 
-///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 ///If no axis is provided, it uses the default axis for this constraint.
 void b3Point2PointConstraint::setParam(int num, b3Scalar value, int axis)
 {
-	if(axis != -1)
+	if (axis != -1)
 	{
 		b3AssertConstrParams(0);
 	}
 	else
 	{
-		switch(num)
+		switch (num)
 		{
-			case B3_CONSTRAINT_ERP :
-			case B3_CONSTRAINT_STOP_ERP :
-				m_erp = value; 
+			case B3_CONSTRAINT_ERP:
+			case B3_CONSTRAINT_STOP_ERP:
+				m_erp = value;
 				m_flags |= B3_P2P_FLAGS_ERP;
 				break;
-			case B3_CONSTRAINT_CFM :
-			case B3_CONSTRAINT_STOP_CFM :
-				m_cfm = value; 
+			case B3_CONSTRAINT_CFM:
+			case B3_CONSTRAINT_STOP_CFM:
+				m_cfm = value;
 				m_flags |= B3_P2P_FLAGS_CFM;
 				break;
-			default: 
+			default:
 				b3AssertConstrParams(0);
 		}
 	}
 }
 
 ///return the local value of parameter
-b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const 
+b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const
 {
 	b3Scalar retVal(B3_INFINITY);
-	if(axis != -1)
+	if (axis != -1)
 	{
 		b3AssertConstrParams(0);
 	}
 	else
 	{
-		switch(num)
+		switch (num)
 		{
-			case B3_CONSTRAINT_ERP :
-			case B3_CONSTRAINT_STOP_ERP :
+			case B3_CONSTRAINT_ERP:
+			case B3_CONSTRAINT_STOP_ERP:
 				b3AssertConstrParams(m_flags & B3_P2P_FLAGS_ERP);
-				retVal = m_erp; 
+				retVal = m_erp;
 				break;
-			case B3_CONSTRAINT_CFM :
-			case B3_CONSTRAINT_STOP_CFM :
+			case B3_CONSTRAINT_CFM:
+			case B3_CONSTRAINT_STOP_CFM:
 				b3AssertConstrParams(m_flags & B3_P2P_FLAGS_CFM);
-				retVal = m_cfm; 
+				retVal = m_cfm;
 				break;
-			default: 
+			default:
 				b3AssertConstrParams(0);
 		}
 	}
 	return retVal;
 }
-	
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h
index 681b487334..14762a3e35 100644
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h
@@ -22,26 +22,24 @@ subject to the following restrictions:
 
 class b3RigidBody;
 
-
 #ifdef B3_USE_DOUBLE_PRECISION
-#define b3Point2PointConstraintData	b3Point2PointConstraintDoubleData
-#define b3Point2PointConstraintDataName	"b3Point2PointConstraintDoubleData"
+#define b3Point2PointConstraintData b3Point2PointConstraintDoubleData
+#define b3Point2PointConstraintDataName "b3Point2PointConstraintDoubleData"
 #else
-#define b3Point2PointConstraintData	b3Point2PointConstraintFloatData
-#define b3Point2PointConstraintDataName	"b3Point2PointConstraintFloatData"
-#endif //B3_USE_DOUBLE_PRECISION
+#define b3Point2PointConstraintData b3Point2PointConstraintFloatData
+#define b3Point2PointConstraintDataName "b3Point2PointConstraintFloatData"
+#endif  //B3_USE_DOUBLE_PRECISION
 
-struct	b3ConstraintSetting
+struct b3ConstraintSetting
 {
-	b3ConstraintSetting()	:
-		m_tau(b3Scalar(0.3)),
-		m_damping(b3Scalar(1.)),
-		m_impulseClamp(b3Scalar(0.))
+	b3ConstraintSetting() : m_tau(b3Scalar(0.3)),
+							m_damping(b3Scalar(1.)),
+							m_impulseClamp(b3Scalar(0.))
 	{
 	}
-	b3Scalar		m_tau;
-	b3Scalar		m_damping;
-	b3Scalar		m_impulseClamp;
+	b3Scalar m_tau;
+	b3Scalar m_damping;
+	b3Scalar m_impulseClamp;
 };
 
 enum b3Point2PointFlags
@@ -51,47 +49,45 @@ enum b3Point2PointFlags
 };
 
 /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
-B3_ATTRIBUTE_ALIGNED16(class) b3Point2PointConstraint : public b3TypedConstraint
+B3_ATTRIBUTE_ALIGNED16(class)
+b3Point2PointConstraint : public b3TypedConstraint
 {
 #ifdef IN_PARALLELL_SOLVER
 public:
 #endif
-	
-	b3Vector3	m_pivotInA;
-	b3Vector3	m_pivotInB;
-	
-	int			m_flags;
-	b3Scalar	m_erp;
-	b3Scalar	m_cfm;
-	
-public:
 
-	B3_DECLARE_ALIGNED_ALLOCATOR();
+	b3Vector3 m_pivotInA;
+	b3Vector3 m_pivotInB;
 
-	b3ConstraintSetting	m_setting;
+	int m_flags;
+	b3Scalar m_erp;
+	b3Scalar m_cfm;
 
-	b3Point2PointConstraint(int  rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB);
+public:
+	B3_DECLARE_ALIGNED_ALLOCATOR();
 
-	//b3Point2PointConstraint(int  rbA,const b3Vector3& pivotInA);
+	b3ConstraintSetting m_setting;
 
+	b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB);
 
+	//b3Point2PointConstraint(int  rbA,const b3Vector3& pivotInA);
 
-	virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
+	virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
 
-	void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
+	void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
 
-	virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies);
+	virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
 
-	void getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans);
+	void getInfo2NonVirtual(b3ConstraintInfo2 * info, const b3Transform& body0_trans, const b3Transform& body1_trans);
 
-	void	updateRHS(b3Scalar	timeStep);
+	void updateRHS(b3Scalar timeStep);
 
-	void	setPivotA(const b3Vector3& pivotA)
+	void setPivotA(const b3Vector3& pivotA)
 	{
 		m_pivotInA = pivotA;
 	}
 
-	void	setPivotB(const b3Vector3& pivotB)
+	void setPivotB(const b3Vector3& pivotB)
 	{
 		m_pivotInB = pivotB;
 	}
@@ -106,34 +102,32 @@ public:
 		return m_pivotInB;
 	}
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 	///If no axis is provided, it uses the default axis for this constraint.
-	virtual	void	setParam(int num, b3Scalar value, int axis = -1);
+	virtual void setParam(int num, b3Scalar value, int axis = -1);
 	///return the local value of parameter
-	virtual	b3Scalar getParam(int num, int axis = -1) const;
+	virtual b3Scalar getParam(int num, int axis = -1) const;
 
-//	virtual	int	calculateSerializeBufferSize() const;
+	//	virtual	int	calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
-//	virtual	const char*	serialize(void* dataBuffer, b3Serializer* serializer) const;
-
-
+	//	virtual	const char*	serialize(void* dataBuffer, b3Serializer* serializer) const;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	b3Point2PointConstraintFloatData
+struct b3Point2PointConstraintFloatData
 {
-	b3TypedConstraintData	m_typeConstraintData;
-	b3Vector3FloatData	m_pivotInA;
-	b3Vector3FloatData	m_pivotInB;
+	b3TypedConstraintData m_typeConstraintData;
+	b3Vector3FloatData m_pivotInA;
+	b3Vector3FloatData m_pivotInB;
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
-struct	b3Point2PointConstraintDoubleData
+struct b3Point2PointConstraintDoubleData
 {
-	b3TypedConstraintData	m_typeConstraintData;
-	b3Vector3DoubleData	m_pivotInA;
-	b3Vector3DoubleData	m_pivotInB;
+	b3TypedConstraintData m_typeConstraintData;
+	b3Vector3DoubleData m_pivotInA;
+	b3Vector3DoubleData m_pivotInB;
 };
 
 /*
@@ -156,4 +150,4 @@ B3_FORCE_INLINE	const char*	b3Point2PointConstraint::serialize(void* dataBuffer,
 }
 */
 
-#endif //B3_POINT2POINTCONSTRAINT_H
+#endif  //B3_POINT2POINTCONSTRAINT_H
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h
index 0049317d98..196d0e5793 100644
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h
@@ -16,7 +16,6 @@ subject to the following restrictions:
 #ifndef B3_SOLVER_BODY_H
 #define B3_SOLVER_BODY_H
 
-
 #include "Bullet3Common/b3Vector3.h"
 #include "Bullet3Common/b3Matrix3x3.h"
 
@@ -26,110 +25,104 @@ subject to the following restrictions:
 ///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
 #ifdef B3_USE_SSE
 #define USE_SIMD 1
-#endif //
-
+#endif  //
 
 #ifdef USE_SIMD
 
-struct	b3SimdScalar
+struct b3SimdScalar
 {
-	B3_FORCE_INLINE	b3SimdScalar()
+	B3_FORCE_INLINE b3SimdScalar()
 	{
-
 	}
 
-	B3_FORCE_INLINE	b3SimdScalar(float	fl)
-	:m_vec128 (_mm_set1_ps(fl))
+	B3_FORCE_INLINE b3SimdScalar(float fl)
+		: m_vec128(_mm_set1_ps(fl))
 	{
 	}
 
-	B3_FORCE_INLINE	b3SimdScalar(__m128 v128)
-		:m_vec128(v128)
+	B3_FORCE_INLINE b3SimdScalar(__m128 v128)
+		: m_vec128(v128)
 	{
 	}
-	union
-	{
-		__m128		m_vec128;
-		float		m_floats[4];
-		float		x,y,z,w;
-		int			m_ints[4];
-		b3Scalar	m_unusedPadding;
+	union {
+		__m128 m_vec128;
+		float m_floats[4];
+		float x, y, z, w;
+		int m_ints[4];
+		b3Scalar m_unusedPadding;
 	};
-	B3_FORCE_INLINE	__m128	get128()
+	B3_FORCE_INLINE __m128 get128()
 	{
 		return m_vec128;
 	}
 
-	B3_FORCE_INLINE	const __m128	get128() const
+	B3_FORCE_INLINE const __m128 get128() const
 	{
 		return m_vec128;
 	}
 
-	B3_FORCE_INLINE	void	set128(__m128 v128)
+	B3_FORCE_INLINE void set128(__m128 v128)
 	{
 		m_vec128 = v128;
 	}
 
-	B3_FORCE_INLINE	operator       __m128()       
-	{ 
-		return m_vec128; 
-	}
-	B3_FORCE_INLINE	operator const __m128() const 
-	{ 
-		return m_vec128; 
+	B3_FORCE_INLINE operator __m128()
+	{
+		return m_vec128;
 	}
-	
-	B3_FORCE_INLINE	operator float() const 
-	{ 
-		return m_floats[0]; 
+	B3_FORCE_INLINE operator const __m128() const
+	{
+		return m_vec128;
 	}
 
+	B3_FORCE_INLINE operator float() const
+	{
+		return m_floats[0];
+	}
 };
 
 ///@brief Return the elementwise product of two b3SimdScalar
-B3_FORCE_INLINE b3SimdScalar 
-operator*(const b3SimdScalar& v1, const b3SimdScalar& v2) 
+B3_FORCE_INLINE b3SimdScalar
+operator*(const b3SimdScalar& v1, const b3SimdScalar& v2)
 {
-	return b3SimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
+	return b3SimdScalar(_mm_mul_ps(v1.get128(), v2.get128()));
 }
 
 ///@brief Return the elementwise product of two b3SimdScalar
-B3_FORCE_INLINE b3SimdScalar 
-operator+(const b3SimdScalar& v1, const b3SimdScalar& v2) 
+B3_FORCE_INLINE b3SimdScalar
+operator+(const b3SimdScalar& v1, const b3SimdScalar& v2)
 {
-	return b3SimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
+	return b3SimdScalar(_mm_add_ps(v1.get128(), v2.get128()));
 }
 
-
 #else
 #define b3SimdScalar b3Scalar
 #endif
 
 ///The b3SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
-B3_ATTRIBUTE_ALIGNED16 (struct)	b3SolverBody
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3SolverBody
 {
 	B3_DECLARE_ALIGNED_ALLOCATOR();
-	b3Transform		m_worldTransform;
-	b3Vector3		m_deltaLinearVelocity;
-	b3Vector3		m_deltaAngularVelocity;
-	b3Vector3		m_angularFactor;
-	b3Vector3		m_linearFactor;
-	b3Vector3		m_invMass;
-	b3Vector3		m_pushVelocity;
-	b3Vector3		m_turnVelocity;
-	b3Vector3		m_linearVelocity;
-	b3Vector3		m_angularVelocity;
-
-	union 
-	{
-		void*	m_originalBody;
-		int		m_originalBodyIndex;
+	b3Transform m_worldTransform;
+	b3Vector3 m_deltaLinearVelocity;
+	b3Vector3 m_deltaAngularVelocity;
+	b3Vector3 m_angularFactor;
+	b3Vector3 m_linearFactor;
+	b3Vector3 m_invMass;
+	b3Vector3 m_pushVelocity;
+	b3Vector3 m_turnVelocity;
+	b3Vector3 m_linearVelocity;
+	b3Vector3 m_angularVelocity;
+
+	union {
+		void* m_originalBody;
+		int m_originalBodyIndex;
 	};
 
 	int padding[3];
 
-
-	void	setWorldTransform(const b3Transform& worldTransform)
+	void setWorldTransform(const b3Transform& worldTransform)
 	{
 		m_worldTransform = worldTransform;
 	}
@@ -138,45 +131,42 @@ B3_ATTRIBUTE_ALIGNED16 (struct)	b3SolverBody
 	{
 		return m_worldTransform;
 	}
-	
-	B3_FORCE_INLINE void	getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const
+
+	B3_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const
 	{
 		if (m_originalBody)
-			velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
+			velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
 		else
-			velocity.setValue(0,0,0);
+			velocity.setValue(0, 0, 0);
 	}
 
-	B3_FORCE_INLINE void	getAngularVelocity(b3Vector3& angVel) const
+	B3_FORCE_INLINE void getAngularVelocity(b3Vector3 & angVel) const
 	{
 		if (m_originalBody)
-			angVel =m_angularVelocity+m_deltaAngularVelocity;
+			angVel = m_angularVelocity + m_deltaAngularVelocity;
 		else
-			angVel.setValue(0,0,0);
+			angVel.setValue(0, 0, 0);
 	}
 
-
 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude)
+	B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude)
 	{
 		if (m_originalBody)
 		{
-			m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
-			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+			m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
+			m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
 		}
 	}
 
-	B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,b3Scalar impulseMagnitude)
+	B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, b3Scalar impulseMagnitude)
 	{
 		if (m_originalBody)
 		{
-			m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor;
-			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+			m_pushVelocity += linearComponent * impulseMagnitude * m_linearFactor;
+			m_turnVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
 		}
 	}
 
-
-
 	const b3Vector3& getDeltaLinearVelocity() const
 	{
 		return m_deltaLinearVelocity;
@@ -187,20 +177,19 @@ B3_ATTRIBUTE_ALIGNED16 (struct)	b3SolverBody
 		return m_deltaAngularVelocity;
 	}
 
-	const b3Vector3& getPushVelocity() const 
+	const b3Vector3& getPushVelocity() const
 	{
 		return m_pushVelocity;
 	}
 
-	const b3Vector3& getTurnVelocity() const 
+	const b3Vector3& getTurnVelocity() const
 	{
 		return m_turnVelocity;
 	}
 
-
 	////////////////////////////////////////////////
 	///some internal methods, don't use them
-		
+
 	b3Vector3& internalGetDeltaLinearVelocity()
 	{
 		return m_deltaLinearVelocity;
@@ -225,7 +214,7 @@ B3_ATTRIBUTE_ALIGNED16 (struct)	b3SolverBody
 	{
 		m_invMass = invMass;
 	}
-	
+
 	b3Vector3& internalGetPushVelocity()
 	{
 		return m_pushVelocity;
@@ -236,67 +225,57 @@ B3_ATTRIBUTE_ALIGNED16 (struct)	b3SolverBody
 		return m_turnVelocity;
 	}
 
-	B3_FORCE_INLINE void	internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const
+	B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const
 	{
-		velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
+		velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos);
 	}
 
-	B3_FORCE_INLINE void	internalGetAngularVelocity(b3Vector3& angVel) const
+	B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3 & angVel) const
 	{
-		angVel = m_angularVelocity+m_deltaAngularVelocity;
+		angVel = m_angularVelocity + m_deltaAngularVelocity;
 	}
 
-
 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude)
+	B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude)
 	{
 		//if (m_originalBody)
 		{
-			m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
-			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+			m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
+			m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor);
 		}
 	}
-		
-	
-	
 
-	void	writebackVelocity()
+	void writebackVelocity()
 	{
 		//if (m_originalBody>=0)
 		{
-			m_linearVelocity +=m_deltaLinearVelocity;
+			m_linearVelocity += m_deltaLinearVelocity;
 			m_angularVelocity += m_deltaAngularVelocity;
-			
+
 			//m_originalBody->setCompanionId(-1);
 		}
 	}
 
-
-	void	writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp)
+	void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp)
 	{
-        (void) timeStep;
+		(void)timeStep;
 		if (m_originalBody)
 		{
 			m_linearVelocity += m_deltaLinearVelocity;
 			m_angularVelocity += m_deltaAngularVelocity;
-			
+
 			//correct the position/orientation based on push/turn recovery
 			b3Transform newTransform;
-			if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0)
+			if (m_pushVelocity[0] != 0.f || m_pushVelocity[1] != 0 || m_pushVelocity[2] != 0 || m_turnVelocity[0] != 0.f || m_turnVelocity[1] != 0 || m_turnVelocity[2] != 0)
 			{
-			//	b3Quaternion orn = m_worldTransform.getRotation();
-				b3TransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
+				//	b3Quaternion orn = m_worldTransform.getRotation();
+				b3TransformUtil::integrateTransform(m_worldTransform, m_pushVelocity, m_turnVelocity * splitImpulseTurnErp, timeStep, newTransform);
 				m_worldTransform = newTransform;
 			}
 			//m_worldTransform.setRotation(orn);
 			//m_originalBody->setCompanionId(-1);
 		}
 	}
-	
-
-
 };
 
-#endif //B3_SOLVER_BODY_H
-
-
+#endif  //B3_SOLVER_BODY_H
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h
index bce83d4608..4927ae4288 100644
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h
@@ -16,7 +16,6 @@ subject to the following restrictions:
 #ifndef B3_SOLVER_CONSTRAINT_H
 #define B3_SOLVER_CONSTRAINT_H
 
-
 #include "Bullet3Common/b3Vector3.h"
 #include "Bullet3Common/b3Matrix3x3.h"
 //#include "b3JacobianEntry.h"
@@ -25,56 +24,50 @@ subject to the following restrictions:
 //#define NO_FRICTION_TANGENTIALS 1
 #include "b3SolverBody.h"
 
-
 ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
-B3_ATTRIBUTE_ALIGNED16 (struct)	b3SolverConstraint
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3SolverConstraint
 {
 	B3_DECLARE_ALIGNED_ALLOCATOR();
 
-	b3Vector3		m_relpos1CrossNormal;
-	b3Vector3		m_contactNormal;
+	b3Vector3 m_relpos1CrossNormal;
+	b3Vector3 m_contactNormal;
 
-	b3Vector3		m_relpos2CrossNormal;
+	b3Vector3 m_relpos2CrossNormal;
 	//b3Vector3		m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
 
-	b3Vector3		m_angularComponentA;
-	b3Vector3		m_angularComponentB;
-	
-	mutable b3SimdScalar	m_appliedPushImpulse;
-	mutable b3SimdScalar	m_appliedImpulse;
+	b3Vector3 m_angularComponentA;
+	b3Vector3 m_angularComponentB;
+
+	mutable b3SimdScalar m_appliedPushImpulse;
+	mutable b3SimdScalar m_appliedImpulse;
 	int m_padding1;
 	int m_padding2;
-	b3Scalar	m_friction;
-	b3Scalar	m_jacDiagABInv;
-	b3Scalar		m_rhs;
-	b3Scalar		m_cfm;
-	
-    b3Scalar		m_lowerLimit;
-	b3Scalar		m_upperLimit;
-	b3Scalar		m_rhsPenetration;
-    union
-	{
-		void*		m_originalContactPoint;
-		b3Scalar	m_unusedPadding4;
+	b3Scalar m_friction;
+	b3Scalar m_jacDiagABInv;
+	b3Scalar m_rhs;
+	b3Scalar m_cfm;
+
+	b3Scalar m_lowerLimit;
+	b3Scalar m_upperLimit;
+	b3Scalar m_rhsPenetration;
+	union {
+		void* m_originalContactPoint;
+		b3Scalar m_unusedPadding4;
 	};
 
-	int	m_overrideNumSolverIterations;
-    int			m_frictionIndex;
+	int m_overrideNumSolverIterations;
+	int m_frictionIndex;
 	int m_solverBodyIdA;
 	int m_solverBodyIdB;
 
-    
-	enum		b3SolverConstraintType
+	enum b3SolverConstraintType
 	{
 		B3_SOLVER_CONTACT_1D = 0,
 		B3_SOLVER_FRICTION_1D
 	};
 };
 
-typedef b3AlignedObjectArray<b3SolverConstraint>	b3ConstraintArray;
-
-
-#endif //B3_SOLVER_CONSTRAINT_H
-
-
+typedef b3AlignedObjectArray<b3SolverConstraint> b3ConstraintArray;
 
+#endif  //B3_SOLVER_CONSTRAINT_H
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp
index 699c481d64..885e277d8c 100644
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp
@@ -13,53 +13,46 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-
 #include "b3TypedConstraint.h"
 //#include "Bullet3Common/b3Serializer.h"
 
-
 #define B3_DEFAULT_DEBUGDRAW_SIZE b3Scalar(0.3f)
 
-
-
-b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA,int rbB)
-:b3TypedObject(type),
-m_userConstraintType(-1),
-m_userConstraintPtr((void*)-1),
-m_breakingImpulseThreshold(B3_INFINITY),
-m_isEnabled(true),
-m_needsFeedback(false),
-m_overrideNumSolverIterations(-1),
-m_rbA(rbA),
-m_rbB(rbB),
-m_appliedImpulse(b3Scalar(0.)),
-m_dbgDrawSize(B3_DEFAULT_DEBUGDRAW_SIZE),
-m_jointFeedback(0)
+b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA, int rbB)
+	: b3TypedObject(type),
+	  m_userConstraintType(-1),
+	  m_userConstraintPtr((void*)-1),
+	  m_breakingImpulseThreshold(B3_INFINITY),
+	  m_isEnabled(true),
+	  m_needsFeedback(false),
+	  m_overrideNumSolverIterations(-1),
+	  m_rbA(rbA),
+	  m_rbB(rbB),
+	  m_appliedImpulse(b3Scalar(0.)),
+	  m_dbgDrawSize(B3_DEFAULT_DEBUGDRAW_SIZE),
+	  m_jointFeedback(0)
 {
 }
 
-
-
-
 b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact)
 {
-	if(lowLim > uppLim)
+	if (lowLim > uppLim)
 	{
 		return b3Scalar(1.0f);
 	}
-	else if(lowLim == uppLim)
+	else if (lowLim == uppLim)
 	{
 		return b3Scalar(0.0f);
 	}
 	b3Scalar lim_fact = b3Scalar(1.0f);
 	b3Scalar delta_max = vel / timeFact;
-	if(delta_max < b3Scalar(0.0f))
+	if (delta_max < b3Scalar(0.0f))
 	{
-		if((pos >= lowLim) && (pos < (lowLim - delta_max)))
+		if ((pos >= lowLim) && (pos < (lowLim - delta_max)))
 		{
 			lim_fact = (lowLim - pos) / delta_max;
 		}
-		else if(pos  < lowLim)
+		else if (pos < lowLim)
 		{
 			lim_fact = b3Scalar(0.0f);
 		}
@@ -68,13 +61,13 @@ b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scal
 			lim_fact = b3Scalar(1.0f);
 		}
 	}
-	else if(delta_max > b3Scalar(0.0f))
+	else if (delta_max > b3Scalar(0.0f))
 	{
-		if((pos <= uppLim) && (pos > (uppLim - delta_max)))
+		if ((pos <= uppLim) && (pos > (uppLim - delta_max)))
 		{
 			lim_fact = (uppLim - pos) / delta_max;
 		}
-		else if(pos  > uppLim)
+		else if (pos > uppLim)
 		{
 			lim_fact = b3Scalar(0.0f);
 		}
@@ -85,18 +78,16 @@ b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scal
 	}
 	else
 	{
-			lim_fact = b3Scalar(0.0f);
+		lim_fact = b3Scalar(0.0f);
 	}
 	return lim_fact;
 }
 
-
-
 void b3AngularLimit::set(b3Scalar low, b3Scalar high, b3Scalar _softness, b3Scalar _biasFactor, b3Scalar _relaxationFactor)
 {
 	m_halfRange = (high - low) / 2.0f;
 	m_center = b3NormalizeAngle(low + m_halfRange);
-	m_softness =  _softness;
+	m_softness = _softness;
 	m_biasFactor = _biasFactor;
 	m_relaxationFactor = _relaxationFactor;
 }
@@ -113,7 +104,7 @@ void b3AngularLimit::test(const b3Scalar angle)
 		if (deviation < -m_halfRange)
 		{
 			m_solveLimit = true;
-			m_correction = - (deviation + m_halfRange);
+			m_correction = -(deviation + m_halfRange);
 			m_sign = +1.0f;
 		}
 		else if (deviation > m_halfRange)
@@ -125,7 +116,6 @@ void b3AngularLimit::test(const b3Scalar angle)
 	}
 }
 
-
 b3Scalar b3AngularLimit::getError() const
 {
 	return m_correction * m_sign;
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h
index cf9cec0d5e..f74aec4d3c 100644
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h
@@ -16,7 +16,6 @@ subject to the following restrictions:
 #ifndef B3_TYPED_CONSTRAINT_H
 #define B3_TYPED_CONSTRAINT_H
 
-
 #include "Bullet3Common/b3Scalar.h"
 #include "b3SolverConstraint.h"
 
@@ -25,7 +24,7 @@ class b3Serializer;
 //Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
 enum b3TypedConstraintType
 {
-	B3_POINT2POINT_CONSTRAINT_TYPE=3,
+	B3_POINT2POINT_CONSTRAINT_TYPE = 3,
 	B3_HINGE_CONSTRAINT_TYPE,
 	B3_CONETWIST_CONSTRAINT_TYPE,
 	B3_D6_CONSTRAINT_TYPE,
@@ -37,92 +36,86 @@ enum b3TypedConstraintType
 	B3_MAX_CONSTRAINT_TYPE
 };
 
-
 enum b3ConstraintParams
 {
-	B3_CONSTRAINT_ERP=1,
+	B3_CONSTRAINT_ERP = 1,
 	B3_CONSTRAINT_STOP_ERP,
 	B3_CONSTRAINT_CFM,
 	B3_CONSTRAINT_STOP_CFM
 };
 
 #if 1
-	#define b3AssertConstrParams(_par) b3Assert(_par) 
+#define b3AssertConstrParams(_par) b3Assert(_par)
 #else
-	#define b3AssertConstrParams(_par)
+#define b3AssertConstrParams(_par)
 #endif
 
-
-B3_ATTRIBUTE_ALIGNED16(struct)	b3JointFeedback
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3JointFeedback
 {
-	b3Vector3	m_appliedForceBodyA;
-	b3Vector3	m_appliedTorqueBodyA;
-	b3Vector3	m_appliedForceBodyB;
-	b3Vector3	m_appliedTorqueBodyB;
+	b3Vector3 m_appliedForceBodyA;
+	b3Vector3 m_appliedTorqueBodyA;
+	b3Vector3 m_appliedForceBodyB;
+	b3Vector3 m_appliedTorqueBodyB;
 };
 
-
 struct b3RigidBodyData;
 
-
 ///TypedConstraint is the baseclass for Bullet constraints and vehicles
-B3_ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public b3TypedObject
+B3_ATTRIBUTE_ALIGNED16(class)
+b3TypedConstraint : public b3TypedObject
 {
-	int	m_userConstraintType;
+	int m_userConstraintType;
 
-	union
-	{
-		int	m_userConstraintId;
+	union {
+		int m_userConstraintId;
 		void* m_userConstraintPtr;
 	};
 
-	b3Scalar	m_breakingImpulseThreshold;
-	bool		m_isEnabled;
-	bool		m_needsFeedback;
-	int			m_overrideNumSolverIterations;
+	b3Scalar m_breakingImpulseThreshold;
+	bool m_isEnabled;
+	bool m_needsFeedback;
+	int m_overrideNumSolverIterations;
 
-
-	b3TypedConstraint&	operator=(b3TypedConstraint&	other)
+	b3TypedConstraint& operator=(b3TypedConstraint& other)
 	{
 		b3Assert(0);
-		(void) other;
+		(void)other;
 		return *this;
 	}
 
 protected:
-	int				m_rbA;
-	int				m_rbB;
-	b3Scalar	m_appliedImpulse;
-	b3Scalar	m_dbgDrawSize;
-	b3JointFeedback*	m_jointFeedback;
+	int m_rbA;
+	int m_rbB;
+	b3Scalar m_appliedImpulse;
+	b3Scalar m_dbgDrawSize;
+	b3JointFeedback* m_jointFeedback;
 
 	///internal method used by the constraint solver, don't use them directly
 	b3Scalar getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact);
-	
 
 public:
-
 	B3_DECLARE_ALIGNED_ALLOCATOR();
 
-	virtual ~b3TypedConstraint() {};
-	b3TypedConstraint(b3TypedConstraintType type, int bodyA,int bodyB);
+	virtual ~b3TypedConstraint(){};
+	b3TypedConstraint(b3TypedConstraintType type, int bodyA, int bodyB);
 
-	struct b3ConstraintInfo1 {
-		int m_numConstraintRows,nub;
+	struct b3ConstraintInfo1
+	{
+		int m_numConstraintRows, nub;
 	};
 
-	
-
-	struct b3ConstraintInfo2 {
+	struct b3ConstraintInfo2
+	{
 		// integrator parameters: frames per second (1/stepsize), default error
 		// reduction parameter (0..1).
-		b3Scalar fps,erp;
+		b3Scalar fps, erp;
 
 		// for the first and second body, pointers to two (linear and angular)
 		// n*3 jacobian sub matrices, stored by rows. these matrices will have
 		// been initialized to 0 on entry. if the second body is zero then the
 		// J2xx pointers may be 0.
-		b3Scalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
+		b3Scalar *m_J1linearAxis, *m_J1angularAxis, *m_J2linearAxis, *m_J2angularAxis;
 
 		// elements to jump from one row to the next in J's
 		int rowskip;
@@ -130,24 +123,24 @@ public:
 		// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
 		// "constraint force mixing" vector. c is set to zero on entry, cfm is
 		// set to a constant value (typically very small or zero) value on entry.
-		b3Scalar *m_constraintError,*cfm;
+		b3Scalar *m_constraintError, *cfm;
 
 		// lo and hi limits for variables (set to -/+ infinity on entry).
-		b3Scalar *m_lowerLimit,*m_upperLimit;
+		b3Scalar *m_lowerLimit, *m_upperLimit;
 
 		// findex vector for variables. see the LCP solver interface for a
 		// description of what this does. this is set to -1 on entry.
 		// note that the returned indexes are relative to the first index of
 		// the constraint.
-		int *findex;
+		int* findex;
 		// number of solver iterations
 		int m_numIterations;
 
 		//damping of the velocity
-		b3Scalar	m_damping;
+		b3Scalar m_damping;
 	};
 
-	int	getOverrideNumSolverIterations() const
+	int getOverrideNumSolverIterations() const
 	{
 		return m_overrideNumSolverIterations;
 	}
@@ -159,59 +152,55 @@ public:
 		m_overrideNumSolverIterations = overideNumIterations;
 	}
 
-
 	///internal method used by the constraint solver, don't use them directly
-	virtual	void	setupSolverConstraint(b3ConstraintArray& ca, int solverBodyA,int solverBodyB, b3Scalar timeStep)
+	virtual void setupSolverConstraint(b3ConstraintArray & ca, int solverBodyA, int solverBodyB, b3Scalar timeStep)
 	{
-        (void)ca;
-        (void)solverBodyA;
-        (void)solverBodyB;
-        (void)timeStep;
+		(void)ca;
+		(void)solverBodyA;
+		(void)solverBodyB;
+		(void)timeStep;
 	}
-	
+
 	///internal method used by the constraint solver, don't use them directly
-	virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)=0;
+	virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies) = 0;
 
 	///internal method used by the constraint solver, don't use them directly
-	virtual void getInfo2 (b3ConstraintInfo2* info,  const b3RigidBodyData* bodies)=0;
+	virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies) = 0;
 
 	///internal method used by the constraint solver, don't use them directly
-	void	internalSetAppliedImpulse(b3Scalar appliedImpulse)
+	void internalSetAppliedImpulse(b3Scalar appliedImpulse)
 	{
 		m_appliedImpulse = appliedImpulse;
 	}
 	///internal method used by the constraint solver, don't use them directly
-	b3Scalar	internalGetAppliedImpulse()
+	b3Scalar internalGetAppliedImpulse()
 	{
 		return m_appliedImpulse;
 	}
 
-
-	b3Scalar	getBreakingImpulseThreshold() const
+	b3Scalar getBreakingImpulseThreshold() const
 	{
-		return 	m_breakingImpulseThreshold;
+		return m_breakingImpulseThreshold;
 	}
 
-	void	setBreakingImpulseThreshold(b3Scalar threshold)
+	void setBreakingImpulseThreshold(b3Scalar threshold)
 	{
 		m_breakingImpulseThreshold = threshold;
 	}
 
-	bool	isEnabled() const
+	bool isEnabled() const
 	{
 		return m_isEnabled;
 	}
 
-	void	setEnabled(bool enabled)
+	void setEnabled(bool enabled)
 	{
-		m_isEnabled=enabled;
+		m_isEnabled = enabled;
 	}
 
-
 	///internal method used by the constraint solver, don't use them directly
-	virtual	void	solveConstraintObsolete(b3SolverBody& /*bodyA*/,b3SolverBody& /*bodyB*/,b3Scalar	/*timeStep*/) {};
+	virtual void solveConstraintObsolete(b3SolverBody& /*bodyA*/, b3SolverBody& /*bodyB*/, b3Scalar /*timeStep*/){};
 
-	
 	int getRigidBodyA() const
 	{
 		return m_rbA;
@@ -221,8 +210,7 @@ public:
 		return m_rbB;
 	}
 
-
-	int getRigidBodyA() 
+	int getRigidBodyA()
 	{
 		return m_rbA;
 	}
@@ -233,15 +221,15 @@ public:
 
 	int getUserConstraintType() const
 	{
-		return m_userConstraintType ;
+		return m_userConstraintType;
 	}
 
-	void	setUserConstraintType(int userConstraintType)
+	void setUserConstraintType(int userConstraintType)
 	{
 		m_userConstraintType = userConstraintType;
 	};
 
-	void	setUserConstraintId(int uid)
+	void setUserConstraintId(int uid)
 	{
 		m_userConstraintId = uid;
 	}
@@ -251,17 +239,17 @@ public:
 		return m_userConstraintId;
 	}
 
-	void	setUserConstraintPtr(void* ptr)
+	void setUserConstraintPtr(void* ptr)
 	{
 		m_userConstraintPtr = ptr;
 	}
 
-	void*	getUserConstraintPtr()
+	void* getUserConstraintPtr()
 	{
 		return m_userConstraintPtr;
 	}
 
-	void	setJointFeedback(b3JointFeedback* jointFeedback)
+	void setJointFeedback(b3JointFeedback * jointFeedback)
 	{
 		m_jointFeedback = jointFeedback;
 	}
@@ -276,37 +264,36 @@ public:
 		return m_jointFeedback;
 	}
 
-
 	int getUid() const
 	{
-		return m_userConstraintId;   
-	} 
+		return m_userConstraintId;
+	}
 
-	bool	needsFeedback() const
+	bool needsFeedback() const
 	{
 		return m_needsFeedback;
 	}
 
 	///enableFeedback will allow to read the applied linear and angular impulse
 	///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
-	void	enableFeedback(bool needsFeedback)
+	void enableFeedback(bool needsFeedback)
 	{
 		m_needsFeedback = needsFeedback;
 	}
 
-	///getAppliedImpulse is an estimated total applied impulse. 
+	///getAppliedImpulse is an estimated total applied impulse.
 	///This feedback could be used to determine breaking constraints or playing sounds.
-	b3Scalar	getAppliedImpulse() const
+	b3Scalar getAppliedImpulse() const
 	{
 		b3Assert(m_needsFeedback);
 		return m_appliedImpulse;
 	}
 
-	b3TypedConstraintType getConstraintType () const
+	b3TypedConstraintType getConstraintType() const
 	{
 		return b3TypedConstraintType(m_objectType);
 	}
-	
+
 	void setDbgDrawSize(b3Scalar dbgDrawSize)
 	{
 		m_dbgDrawSize = dbgDrawSize;
@@ -316,35 +303,34 @@ public:
 		return m_dbgDrawSize;
 	}
 
-	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
 	///If no axis is provided, it uses the default axis for this constraint.
-	virtual	void	setParam(int num, b3Scalar value, int axis = -1) = 0;
+	virtual void setParam(int num, b3Scalar value, int axis = -1) = 0;
 
 	///return the local value of parameter
-	virtual	b3Scalar getParam(int num, int axis = -1) const = 0;
-	
-//	virtual	int	calculateSerializeBufferSize() const;
+	virtual b3Scalar getParam(int num, int axis = -1) const = 0;
+
+	//	virtual	int	calculateSerializeBufferSize() const;
 
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
 	//virtual	const char*	serialize(void* dataBuffer, b3Serializer* serializer) const;
-
 };
 
-// returns angle in range [-B3_2_PI, B3_2_PI], closest to one of the limits 
+// returns angle in range [-B3_2_PI, B3_2_PI], closest to one of the limits
 // all arguments should be normalized angles (i.e. in range [-B3_PI, B3_PI])
 B3_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar angleLowerLimitInRadians, b3Scalar angleUpperLimitInRadians)
 {
-	if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
+	if (angleLowerLimitInRadians >= angleUpperLimitInRadians)
 	{
 		return angleInRadians;
 	}
-	else if(angleInRadians < angleLowerLimitInRadians)
+	else if (angleInRadians < angleLowerLimitInRadians)
 	{
 		b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleLowerLimitInRadians - angleInRadians));
 		b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleUpperLimitInRadians - angleInRadians));
 		return (diffLo < diffHi) ? angleInRadians : (angleInRadians + B3_2_PI);
 	}
-	else if(angleInRadians > angleUpperLimitInRadians)
+	else if (angleInRadians > angleUpperLimitInRadians)
 	{
 		b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleInRadians - angleUpperLimitInRadians));
 		b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleInRadians - angleLowerLimitInRadians));
@@ -356,6 +342,7 @@ B3_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar
 	}
 }
 
+// clang-format off
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	b3TypedConstraintData
 {
@@ -379,17 +366,18 @@ struct	b3TypedConstraintData
 	
 };
 
+// clang-format on
+
 /*B3_FORCE_INLINE	int	b3TypedConstraint::calculateSerializeBufferSize() const
 {
 	return sizeof(b3TypedConstraintData);
 }
 */
 
-
 class b3AngularLimit
 {
 private:
-	b3Scalar 
+	b3Scalar
 		m_center,
 		m_halfRange,
 		m_softness,
@@ -404,15 +392,16 @@ private:
 public:
 	/// Default constructor initializes limit as inactive, allowing free constraint movement
 	b3AngularLimit()
-		:m_center(0.0f),
-		m_halfRange(-1.0f),
-		m_softness(0.9f),
-		m_biasFactor(0.3f),
-		m_relaxationFactor(1.0f),
-		m_correction(0.0f),
-		m_sign(0.0f),
-		m_solveLimit(false)
-	{}
+		: m_center(0.0f),
+		  m_halfRange(-1.0f),
+		  m_softness(0.9f),
+		  m_biasFactor(0.3f),
+		  m_relaxationFactor(1.0f),
+		  m_correction(0.0f),
+		  m_sign(0.0f),
+		  m_solveLimit(false)
+	{
+	}
 
 	/// Sets all limit's parameters.
 	/// When low > high limit becomes inactive.
@@ -441,13 +430,13 @@ public:
 		return m_relaxationFactor;
 	}
 
-	/// Returns correction value evaluated when test() was invoked 
+	/// Returns correction value evaluated when test() was invoked
 	inline b3Scalar getCorrection() const
 	{
 		return m_correction;
 	}
 
-	/// Returns sign value evaluated when test() was invoked 
+	/// Returns sign value evaluated when test() was invoked
 	inline b3Scalar getSign() const
 	{
 		return m_sign;
@@ -475,9 +464,6 @@ public:
 	b3Scalar getLow() const;
 
 	b3Scalar getHigh() const;
-
 };
 
-
-
-#endif //B3_TYPED_CONSTRAINT_H
+#endif  //B3_TYPED_CONSTRAINT_H
diff --git a/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp b/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp
index fbc84cc28d..f1080d9d5e 100644
--- a/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp
+++ b/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp
@@ -11,7 +11,6 @@
 #include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
 #include "Bullet3Dynamics/shared/b3Inertia.h"
 
-
 struct b3CpuRigidBodyPipelineInternalData
 {
 	b3AlignedObjectArray<b3RigidBodyData> m_rigidBodies;
@@ -22,7 +21,6 @@ struct b3CpuRigidBodyPipelineInternalData
 	b3CpuNarrowPhase* m_np;
 	b3Config m_config;
 };
-	
 
 b3CpuRigidBodyPipeline::b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config)
 {
@@ -39,49 +37,43 @@ b3CpuRigidBodyPipeline::~b3CpuRigidBodyPipeline()
 
 void b3CpuRigidBodyPipeline::updateAabbWorldSpace()
 {
-
-	for (int i=0;i<this->getNumBodies();i++)
+	for (int i = 0; i < this->getNumBodies(); i++)
 	{
 		b3RigidBodyData* body = &m_data->m_rigidBodies[i];
 		b3Float4 position = body->m_pos;
-		b3Quat	orientation = body->m_quat;
+		b3Quat orientation = body->m_quat;
 
 		int collidableIndex = body->m_collidableIdx;
 		b3Collidable& collidable = m_data->m_np->getCollidableCpu(collidableIndex);
 		int shapeIndex = collidable.m_shapeIndex;
-		
-		if (shapeIndex>=0)
-		{
-			
 
+		if (shapeIndex >= 0)
+		{
 			b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(shapeIndex);
 			b3Aabb& worldAabb = m_data->m_aabbWorldSpace[i];
-			float margin=0.f;
-			b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&worldAabb.m_minVec,&worldAabb.m_maxVec);
-			m_data->m_bp->setAabb(i,worldAabb.m_minVec,worldAabb.m_maxVec,0);
+			float margin = 0.f;
+			b3TransformAabb2(localAabb.m_minVec, localAabb.m_maxVec, margin, position, orientation, &worldAabb.m_minVec, &worldAabb.m_maxVec);
+			m_data->m_bp->setAabb(i, worldAabb.m_minVec, worldAabb.m_maxVec, 0);
 		}
 	}
 }
 
-void	b3CpuRigidBodyPipeline::computeOverlappingPairs()
+void b3CpuRigidBodyPipeline::computeOverlappingPairs()
 {
 	int numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
 	m_data->m_bp->calculateOverlappingPairs();
 	numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
-	printf("numPairs=%d\n",numPairs);
+	printf("numPairs=%d\n", numPairs);
 }
 
 void b3CpuRigidBodyPipeline::computeContactPoints()
 {
-	
 	b3AlignedObjectArray<b3Int4>& pairs = m_data->m_bp->getOverlappingPairCache()->getOverlappingPairArray();
-	
-	m_data->m_np->computeContacts(pairs,m_data->m_aabbWorldSpace, m_data->m_rigidBodies);
 
+	m_data->m_np->computeContacts(pairs, m_data->m_aabbWorldSpace, m_data->m_rigidBodies);
 }
-void	b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
+void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
 {
-	
 	//update world space aabb's
 	updateAabbWorldSpace();
 
@@ -92,73 +84,71 @@ void	b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
 	computeContactPoints();
 
 	//solve contacts
-	
+
 	//update transforms
 	integrate(deltaTime);
-	
-	
 }
 
-
-static	inline	float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1, 
-					 const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1)
+static inline float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1,
+								 const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1)
 {
 	return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1);
 }
 
-
-static	inline	void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1,
-							 b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1)
+static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1,
+										 b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1)
 {
 	linear = -n;
 	angular0 = -b3Cross(r0, n);
 	angular1 = b3Cross(r1, n);
 }
 
-
-
-static inline void b3SolveContact(b3ContactConstraint4& cs, 
-	const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
-	const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, 
-	float maxRambdaDt[4], float minRambdaDt[4])
+static inline void b3SolveContact(b3ContactConstraint4& cs,
+								  const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
+								  const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
+								  float maxRambdaDt[4], float minRambdaDt[4])
 {
-
-	b3Vector3 dLinVelA; dLinVelA.setZero();
-	b3Vector3 dAngVelA; dAngVelA.setZero();
-	b3Vector3 dLinVelB; dLinVelB.setZero();
-	b3Vector3 dAngVelB; dAngVelB.setZero();
-
-	for(int ic=0; ic<4; ic++)
+	b3Vector3 dLinVelA;
+	dLinVelA.setZero();
+	b3Vector3 dAngVelA;
+	dAngVelA.setZero();
+	b3Vector3 dLinVelB;
+	dLinVelB.setZero();
+	b3Vector3 dAngVelB;
+	dAngVelB.setZero();
+
+	for (int ic = 0; ic < 4; ic++)
 	{
 		//	dont necessary because this makes change to 0
-		if( cs.m_jacCoeffInv[ic] == 0.f ) continue;
+		if (cs.m_jacCoeffInv[ic] == 0.f) continue;
 
 		{
 			b3Vector3 angular0, angular1, linear;
 			b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA;
 			b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB;
-			b3SetLinearAndAngular( (const b3Vector3 &)-cs.m_linear, (const b3Vector3 &)r0, (const b3Vector3 &)r1, linear, angular0, angular1 );
+			b3SetLinearAndAngular((const b3Vector3&)-cs.m_linear, (const b3Vector3&)r0, (const b3Vector3&)r1, linear, angular0, angular1);
 
-			float rambdaDt = b3CalcRelVel((const b3Vector3 &)cs.m_linear,(const b3Vector3 &) -cs.m_linear, angular0, angular1,
-				linVelA, angVelA, linVelB, angVelB ) + cs.m_b[ic];
+			float rambdaDt = b3CalcRelVel((const b3Vector3&)cs.m_linear, (const b3Vector3&)-cs.m_linear, angular0, angular1,
+										  linVelA, angVelA, linVelB, angVelB) +
+							 cs.m_b[ic];
 			rambdaDt *= cs.m_jacCoeffInv[ic];
 
 			{
 				float prevSum = cs.m_appliedRambdaDt[ic];
 				float updated = prevSum;
 				updated += rambdaDt;
-				updated = b3Max( updated, minRambdaDt[ic] );
-				updated = b3Min( updated, maxRambdaDt[ic] );
+				updated = b3Max(updated, minRambdaDt[ic]);
+				updated = b3Min(updated, maxRambdaDt[ic]);
 				rambdaDt = updated - prevSum;
 				cs.m_appliedRambdaDt[ic] = updated;
 			}
 
-			b3Vector3 linImp0 = invMassA*linear*rambdaDt;
-			b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
-			b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
-			b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
+			b3Vector3 linImp0 = invMassA * linear * rambdaDt;
+			b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
+			b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
+			b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
 #ifdef _WIN32
-            b3Assert(_finite(linImp0.getX()));
+			b3Assert(_finite(linImp0.getX()));
 			b3Assert(_finite(linImp1.getX()));
 #endif
 			{
@@ -169,53 +159,46 @@ static inline void b3SolveContact(b3ContactConstraint4& cs,
 			}
 		}
 	}
-
-
 }
 
-
-
-
-
-static inline void b3SolveFriction(b3ContactConstraint4& cs, 
-		const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
-		const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, 
-		float maxRambdaDt[4], float minRambdaDt[4])
+static inline void b3SolveFriction(b3ContactConstraint4& cs,
+								   const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
+								   const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
+								   float maxRambdaDt[4], float minRambdaDt[4])
 {
-
-	if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return;
+	if (cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0) return;
 	const b3Vector3& center = (const b3Vector3&)cs.m_center;
 
 	b3Vector3 n = -(const b3Vector3&)cs.m_linear;
 
 	b3Vector3 tangent[2];
 
-	b3PlaneSpace1 (n, tangent[0],tangent[1]);
+	b3PlaneSpace1(n, tangent[0], tangent[1]);
 
 	b3Vector3 angular0, angular1, linear;
 	b3Vector3 r0 = center - posA;
 	b3Vector3 r1 = center - posB;
-	for(int i=0; i<2; i++)
+	for (int i = 0; i < 2; i++)
 	{
-		b3SetLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 );
+		b3SetLinearAndAngular(tangent[i], r0, r1, linear, angular0, angular1);
 		float rambdaDt = b3CalcRelVel(linear, -linear, angular0, angular1,
-			linVelA, angVelA, linVelB, angVelB );
+									  linVelA, angVelA, linVelB, angVelB);
 		rambdaDt *= cs.m_fJacCoeffInv[i];
 
-			{
-				float prevSum = cs.m_fAppliedRambdaDt[i];
-				float updated = prevSum;
-				updated += rambdaDt;
-				updated = b3Max( updated, minRambdaDt[i] );
-				updated = b3Min( updated, maxRambdaDt[i] );
-				rambdaDt = updated - prevSum;
-				cs.m_fAppliedRambdaDt[i] = updated;
-			}
+		{
+			float prevSum = cs.m_fAppliedRambdaDt[i];
+			float updated = prevSum;
+			updated += rambdaDt;
+			updated = b3Max(updated, minRambdaDt[i]);
+			updated = b3Min(updated, maxRambdaDt[i]);
+			rambdaDt = updated - prevSum;
+			cs.m_fAppliedRambdaDt[i] = updated;
+		}
 
-		b3Vector3 linImp0 = invMassA*linear*rambdaDt;
-		b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
-		b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
-		b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
+		b3Vector3 linImp0 = invMassA * linear * rambdaDt;
+		b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
+		b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
+		b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
 #ifdef _WIN32
 		b3Assert(_finite(linImp0.getX()));
 		b3Assert(_finite(linImp1.getX()));
@@ -226,57 +209,45 @@ static inline void b3SolveFriction(b3ContactConstraint4& cs,
 		angVelB += angImp1;
 	}
 
-	{	//	angular damping for point constraint
-		b3Vector3 ab = ( posB - posA ).normalized();
-		b3Vector3 ac = ( center - posA ).normalized();
-		if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
+	{  //	angular damping for point constraint
+		b3Vector3 ab = (posB - posA).normalized();
+		b3Vector3 ac = (center - posA).normalized();
+		if (b3Dot(ab, ac) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
 		{
-			float angNA = b3Dot( n, angVelA );
-			float angNB = b3Dot( n, angVelB );
+			float angNA = b3Dot(n, angVelA);
+			float angNB = b3Dot(n, angVelB);
 
-			angVelA -= (angNA*0.1f)*n;
-			angVelB -= (angNB*0.1f)*n;
+			angVelA -= (angNA * 0.1f) * n;
+			angVelB -= (angNB * 0.1f) * n;
 		}
 	}
-
 }
 
-
-
-
-
-struct b3SolveTask// : public ThreadPool::Task
+struct b3SolveTask  // : public ThreadPool::Task
 {
-	b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies,  
-				b3AlignedObjectArray<b3Inertia>& shapes, 
+	b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies,
+				b3AlignedObjectArray<b3Inertia>& shapes,
 				b3AlignedObjectArray<b3ContactConstraint4>& constraints,
 				int start, int nConstraints,
 				int maxNumBatches,
-				b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx
-				)
-		: m_bodies( bodies ), m_shapes( shapes ), m_constraints( constraints ), 
-		m_wgUsedBodies(wgUsedBodies),m_curWgidx(curWgidx),
-m_start( start ), 
-	m_nConstraints( nConstraints ),
-	m_solveFriction( true ),
-	m_maxNumBatches(maxNumBatches)
-	{}
-
-	unsigned short int getType(){ return 0; }
+				b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx)
+		: m_bodies(bodies), m_shapes(shapes), m_constraints(constraints), m_wgUsedBodies(wgUsedBodies), m_curWgidx(curWgidx), m_start(start), m_nConstraints(nConstraints), m_solveFriction(true), m_maxNumBatches(maxNumBatches)
+	{
+	}
+
+	unsigned short int getType() { return 0; }
 
 	void run(int tIdx)
 	{
 		b3AlignedObjectArray<int> usedBodies;
 		//printf("run..............\n");
 
-		
-		for (int bb=0;bb<m_maxNumBatches;bb++)
+		for (int bb = 0; bb < m_maxNumBatches; bb++)
 		{
 			usedBodies.resize(0);
-			for(int ic=m_nConstraints-1; ic>=0; ic--)
+			for (int ic = m_nConstraints - 1; ic >= 0; ic--)
 			//for(int ic=0; ic<m_nConstraints; ic++)
 			{
-				
 				int i = m_start + ic;
 				if (m_constraints[i].m_batchIdx != bb)
 					continue;
@@ -298,87 +269,80 @@ m_start( start ),
 					//printf("ic(b)=%d, localBatch=%d\n",ic,localBatch);
 				}
 #endif
-				if (aIdx==10)
+				if (aIdx == 10)
 				{
 					//printf("ic(a)=%d, localBatch=%d\n",ic,localBatch);
 				}
-				if (usedBodies.size()<(aIdx+1))
+				if (usedBodies.size() < (aIdx + 1))
 				{
-					usedBodies.resize(aIdx+1,0);
+					usedBodies.resize(aIdx + 1, 0);
 				}
-		
-				if (usedBodies.size()<(bIdx+1))
+
+				if (usedBodies.size() < (bIdx + 1))
 				{
-					usedBodies.resize(bIdx+1,0);
+					usedBodies.resize(bIdx + 1, 0);
 				}
 
 				if (bodyA.m_invMass)
 				{
-					b3Assert(usedBodies[aIdx]==0);
+					b3Assert(usedBodies[aIdx] == 0);
 					usedBodies[aIdx]++;
 				}
-				
+
 				if (bodyB.m_invMass)
 				{
-					b3Assert(usedBodies[bIdx]==0);
+					b3Assert(usedBodies[bIdx] == 0);
 					usedBodies[bIdx]++;
 				}
-				
 
-				if( !m_solveFriction )
+				if (!m_solveFriction)
 				{
-					float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
-					float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
-
-					b3SolveContact( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld, 
-							(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld,
-						maxRambdaDt, minRambdaDt );
+					float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
+					float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};
 
+					b3SolveContact(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
+								   (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
+								   maxRambdaDt, minRambdaDt);
 				}
 				else
 				{
-					float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
-					float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
+					float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
+					float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};
 
 					float sum = 0;
-					for(int j=0; j<4; j++)
+					for (int j = 0; j < 4; j++)
 					{
-						sum +=m_constraints[i].m_appliedRambdaDt[j];
+						sum += m_constraints[i].m_appliedRambdaDt[j];
 					}
 					frictionCoeff = 0.7f;
-					for(int j=0; j<4; j++)
+					for (int j = 0; j < 4; j++)
 					{
-						maxRambdaDt[j] = frictionCoeff*sum;
+						maxRambdaDt[j] = frictionCoeff * sum;
 						minRambdaDt[j] = -maxRambdaDt[j];
 					}
 
-				b3SolveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld, 
-						(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld,
-						maxRambdaDt, minRambdaDt );
-			
+					b3SolveFriction(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
+									(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
+									maxRambdaDt, minRambdaDt);
 				}
 			}
 
 			if (m_wgUsedBodies)
 			{
-				if (m_wgUsedBodies[m_curWgidx].size()<usedBodies.size())
+				if (m_wgUsedBodies[m_curWgidx].size() < usedBodies.size())
 				{
 					m_wgUsedBodies[m_curWgidx].resize(usedBodies.size());
 				}
-				for (int i=0;i<usedBodies.size();i++)
+				for (int i = 0; i < usedBodies.size(); i++)
 				{
 					if (usedBodies[i])
 					{
 						//printf("cell %d uses body %d\n", m_curWgidx,i);
-						m_wgUsedBodies[m_curWgidx][i]=1;
+						m_wgUsedBodies[m_curWgidx][i] = 1;
 					}
 				}
 			}
-
 		}
-
-
-		
 	}
 
 	b3AlignedObjectArray<b3RigidBodyData>& m_bodies;
@@ -397,24 +361,22 @@ void b3CpuRigidBodyPipeline::solveContactConstraints()
 	int m_nIterations = 4;
 
 	b3AlignedObjectArray<b3ContactConstraint4> contactConstraints;
-//	const b3AlignedObjectArray<b3Contact4Data>& contacts = m_data->m_np->getContacts();
+	//	const b3AlignedObjectArray<b3Contact4Data>& contacts = m_data->m_np->getContacts();
 	int n = contactConstraints.size();
 	//convert contacts...
 
-
-	
 	int maxNumBatches = 250;
 
-	for(int iter=0; iter<m_nIterations; iter++)
+	for (int iter = 0; iter < m_nIterations; iter++)
 	{
-		b3SolveTask task( m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0);
+		b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0);
 		task.m_solveFriction = false;
 		task.run(0);
 	}
 
-	for(int iter=0; iter<m_nIterations; iter++)
+	for (int iter = 0; iter < m_nIterations; iter++)
 	{
-		b3SolveTask task( m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0);
+		b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0);
 		task.m_solveFriction = true;
 		task.run(0);
 	}
@@ -422,53 +384,51 @@ void b3CpuRigidBodyPipeline::solveContactConstraints()
 
 void b3CpuRigidBodyPipeline::integrate(float deltaTime)
 {
-	float angDamping=0.f;
-	b3Vector3 gravityAcceleration=b3MakeVector3(0,-9,0);
+	float angDamping = 0.f;
+	b3Vector3 gravityAcceleration = b3MakeVector3(0, -9, 0);
 
 	//integrate transforms (external forces/gravity should be moved into constraint solver)
-	for (int i=0;i<m_data->m_rigidBodies.size();i++)
+	for (int i = 0; i < m_data->m_rigidBodies.size(); i++)
 	{
-		b3IntegrateTransform(&m_data->m_rigidBodies[i],deltaTime,angDamping,gravityAcceleration);
+		b3IntegrateTransform(&m_data->m_rigidBodies[i], deltaTime, angDamping, gravityAcceleration);
 	}
-
 }
 
-int		b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
+int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
 {
 	b3RigidBodyData body;
 	int bodyIndex = m_data->m_rigidBodies.size();
-	body.m_invMass = mass ? 1.f/mass : 0.f;
-	body.m_angVel.setValue(0,0,0);
+	body.m_invMass = mass ? 1.f / mass : 0.f;
+	body.m_angVel.setValue(0, 0, 0);
 	body.m_collidableIdx = collidableIndex;
 	body.m_frictionCoeff = 0.3f;
-	body.m_linVel.setValue(0,0,0);
-	body.m_pos.setValue(position[0],position[1],position[2]);
-	body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]);
+	body.m_linVel.setValue(0, 0, 0);
+	body.m_pos.setValue(position[0], position[1], position[2]);
+	body.m_quat.setValue(orientation[0], orientation[1], orientation[2], orientation[3]);
 	body.m_restituitionCoeff = 0.f;
 
 	m_data->m_rigidBodies.push_back(body);
 
-	
-	if (collidableIndex>=0)
+	if (collidableIndex >= 0)
 	{
 		b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand();
 
 		b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex);
-		b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]);
-		b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]);
-		
+		b3Vector3 localAabbMin = b3MakeVector3(localAabb.m_min[0], localAabb.m_min[1], localAabb.m_min[2]);
+		b3Vector3 localAabbMax = b3MakeVector3(localAabb.m_max[0], localAabb.m_max[1], localAabb.m_max[2]);
+
 		b3Scalar margin = 0.01f;
 		b3Transform t;
 		t.setIdentity();
-		t.setOrigin(b3MakeVector3(position[0],position[1],position[2]));
-		t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
-		b3TransformAabb(localAabbMin,localAabbMax, margin,t,worldAabb.m_minVec,worldAabb.m_maxVec);
-
-		m_data->m_bp->createProxy(worldAabb.m_minVec,worldAabb.m_maxVec,bodyIndex,0,1,1);
-//		b3Vector3 aabbMin,aabbMax;
-	//	m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
+		t.setOrigin(b3MakeVector3(position[0], position[1], position[2]));
+		t.setRotation(b3Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]));
+		b3TransformAabb(localAabbMin, localAabbMax, margin, t, worldAabb.m_minVec, worldAabb.m_maxVec);
 
-	} else
+		m_data->m_bp->createProxy(worldAabb.m_minVec, worldAabb.m_maxVec, bodyIndex, 0, 1, 1);
+		//		b3Vector3 aabbMin,aabbMax;
+		//	m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
+	}
+	else
 	{
 		b3Error("registerPhysicsInstance using invalid collidableIndex\n");
 	}
@@ -476,13 +436,12 @@ int		b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po
 	return bodyIndex;
 }
 
-
 const struct b3RigidBodyData* b3CpuRigidBodyPipeline::getBodyBuffer() const
 {
 	return m_data->m_rigidBodies.size() ? &m_data->m_rigidBodies[0] : 0;
 }
 
-int	b3CpuRigidBodyPipeline::getNumBodies() const
+int b3CpuRigidBodyPipeline::getNumBodies() const
 {
 	return m_data->m_rigidBodies.size();
 }
diff --git a/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.h b/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.h
index 2f3c2ae77e..9c65419f26 100644
--- a/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.h
+++ b/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.h
@@ -16,52 +16,47 @@ subject to the following restrictions:
 #ifndef B3_CPU_RIGIDBODY_PIPELINE_H
 #define B3_CPU_RIGIDBODY_PIPELINE_H
 
-
-
 #include "Bullet3Common/b3AlignedObjectArray.h"
 #include "Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h"
 
 class b3CpuRigidBodyPipeline
 {
 protected:
-	struct b3CpuRigidBodyPipelineInternalData*	m_data;
+	struct b3CpuRigidBodyPipelineInternalData* m_data;
 
 	int allocateCollidable();
 
 public:
-
-
 	b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const struct b3Config& config);
 	virtual ~b3CpuRigidBodyPipeline();
 
-	virtual void	stepSimulation(float deltaTime);
-	virtual void	integrate(float timeStep);
-	virtual void	updateAabbWorldSpace();
-	virtual void	computeOverlappingPairs();
-	virtual void	computeContactPoints();
-	virtual void	solveContactConstraints();
-
-	int		registerConvexPolyhedron(class b3ConvexUtility* convex);
-
-	int		registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData);
-	void	writeAllInstancesToGpu();
-	void	copyConstraintsToHost();
-	void	setGravity(const float* grav);
-	void	reset();
-	
-	int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB,float breakingThreshold);
+	virtual void stepSimulation(float deltaTime);
+	virtual void integrate(float timeStep);
+	virtual void updateAabbWorldSpace();
+	virtual void computeOverlappingPairs();
+	virtual void computeContactPoints();
+	virtual void solveContactConstraints();
+
+	int registerConvexPolyhedron(class b3ConvexUtility* convex);
+
+	int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData);
+	void writeAllInstancesToGpu();
+	void copyConstraintsToHost();
+	void setGravity(const float* grav);
+	void reset();
+
+	int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, float breakingThreshold);
 	int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB, float breakingThreshold);
 	void removeConstraintByUid(int uid);
 
-	void	addConstraint(class b3TypedConstraint* constraint);
-	void	removeConstraint(b3TypedConstraint* constraint);
+	void addConstraint(class b3TypedConstraint* constraint);
+	void removeConstraint(b3TypedConstraint* constraint);
 
-	void	castRays(const b3AlignedObjectArray<b3RayInfo>& rays,	b3AlignedObjectArray<b3RayHit>& hitResults);
+	void castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults);
 
 	const struct b3RigidBodyData* getBodyBuffer() const;
 
-	int	getNumBodies() const;
-
+	int getNumBodies() const;
 };
 
-#endif //B3_CPU_RIGIDBODY_PIPELINE_H
\ No newline at end of file
+#endif  //B3_CPU_RIGIDBODY_PIPELINE_H
\ No newline at end of file
diff --git a/thirdparty/bullet/Bullet3Dynamics/shared/b3ContactConstraint4.h b/thirdparty/bullet/Bullet3Dynamics/shared/b3ContactConstraint4.h
index 68cf65e312..cf2eed0e7c 100644
--- a/thirdparty/bullet/Bullet3Dynamics/shared/b3ContactConstraint4.h
+++ b/thirdparty/bullet/Bullet3Dynamics/shared/b3ContactConstraint4.h
@@ -5,30 +5,27 @@
 
 typedef struct b3ContactConstraint4 b3ContactConstraint4_t;
 
-
 struct b3ContactConstraint4
 {
-
-	b3Float4 m_linear;//normal?
+	b3Float4 m_linear;  //normal?
 	b3Float4 m_worldPos[4];
-	b3Float4 m_center;	//	friction
+	b3Float4 m_center;  //	friction
 	float m_jacCoeffInv[4];
 	float m_b[4];
 	float m_appliedRambdaDt[4];
-	float m_fJacCoeffInv[2];	//	friction
-	float m_fAppliedRambdaDt[2];	//	friction
+	float m_fJacCoeffInv[2];      //	friction
+	float m_fAppliedRambdaDt[2];  //	friction
 
 	unsigned int m_bodyA;
 	unsigned int m_bodyB;
-	int			m_batchIdx;
+	int m_batchIdx;
 	unsigned int m_paddings;
-
 };
 
 //inline	void setFrictionCoeff(float value) { m_linear[3] = value; }
-inline	float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint) 
+inline float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint)
 {
-	return constraint->m_linear.w; 
+	return constraint->m_linear.w;
 }
 
-#endif //B3_CONTACT_CONSTRAINT5_H
+#endif  //B3_CONTACT_CONSTRAINT5_H
diff --git a/thirdparty/bullet/Bullet3Dynamics/shared/b3ConvertConstraint4.h b/thirdparty/bullet/Bullet3Dynamics/shared/b3ConvertConstraint4.h
index 805a2bd3ea..3e72f1c3f2 100644
--- a/thirdparty/bullet/Bullet3Dynamics/shared/b3ConvertConstraint4.h
+++ b/thirdparty/bullet/Bullet3Dynamics/shared/b3ConvertConstraint4.h
@@ -4,89 +4,84 @@
 #include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
 
-
-void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q);
- void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q)
+void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q);
+void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q)
 {
-  if (b3Fabs(n.z) > 0.70710678f) {
-    // choose p in y-z plane
-    float a = n.y*n.y + n.z*n.z;
-    float k = 1.f/sqrt(a);
-    p[0].x = 0;
-	p[0].y = -n.z*k;
-	p[0].z = n.y*k;
-    // set q = n x p
-    q[0].x = a*k;
-	q[0].y = -n.x*p[0].z;
-	q[0].z = n.x*p[0].y;
-  }
-  else {
-    // choose p in x-y plane
-    float a = n.x*n.x + n.y*n.y;
-    float k = 1.f/sqrt(a);
-    p[0].x = -n.y*k;
-	p[0].y = n.x*k;
-	p[0].z = 0;
-    // set q = n x p
-    q[0].x = -n.z*p[0].y;
-	q[0].y = n.z*p[0].x;
-	q[0].z = a*k;
-  }
+	if (b3Fabs(n.z) > 0.70710678f)
+	{
+		// choose p in y-z plane
+		float a = n.y * n.y + n.z * n.z;
+		float k = 1.f / sqrt(a);
+		p[0].x = 0;
+		p[0].y = -n.z * k;
+		p[0].z = n.y * k;
+		// set q = n x p
+		q[0].x = a * k;
+		q[0].y = -n.x * p[0].z;
+		q[0].z = n.x * p[0].y;
+	}
+	else
+	{
+		// choose p in x-y plane
+		float a = n.x * n.x + n.y * n.y;
+		float k = 1.f / sqrt(a);
+		p[0].x = -n.y * k;
+		p[0].y = n.x * k;
+		p[0].z = 0;
+		// set q = n x p
+		q[0].x = -n.z * p[0].y;
+		q[0].y = n.z * p[0].x;
+		q[0].z = a * k;
+	}
 }
 
-
- 
-void setLinearAndAngular( b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1)
+void setLinearAndAngular(b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1)
 {
-	*linear = b3MakeFloat4(n.x,n.y,n.z,0.f);
+	*linear = b3MakeFloat4(n.x, n.y, n.z, 0.f);
 	*angular0 = b3Cross3(r0, n);
 	*angular1 = -b3Cross3(r1, n);
 }
 
-
-float calcRelVel( b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0,
-	b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1 )
+float calcRelVel(b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0,
+				 b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1)
 {
 	return b3Dot3F4(l0, linVel0) + b3Dot3F4(a0, angVel0) + b3Dot3F4(l1, linVel1) + b3Dot3F4(a1, angVel1);
 }
 
-
 float calcJacCoeff(b3Float4ConstArg linear0, b3Float4ConstArg linear1, b3Float4ConstArg angular0, b3Float4ConstArg angular1,
-					float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1)
+				   float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1)
 {
 	//	linear0,1 are normlized
-	float jmj0 = invMass0;//b3Dot3F4(linear0, linear0)*invMass0;
-	float jmj1 = b3Dot3F4(mtMul3(angular0,*invInertia0), angular0);
-	float jmj2 = invMass1;//b3Dot3F4(linear1, linear1)*invMass1;
-	float jmj3 = b3Dot3F4(mtMul3(angular1,*invInertia1), angular1);
-	return -1.f/(jmj0+jmj1+jmj2+jmj3);
+	float jmj0 = invMass0;  //b3Dot3F4(linear0, linear0)*invMass0;
+	float jmj1 = b3Dot3F4(mtMul3(angular0, *invInertia0), angular0);
+	float jmj2 = invMass1;  //b3Dot3F4(linear1, linear1)*invMass1;
+	float jmj3 = b3Dot3F4(mtMul3(angular1, *invInertia1), angular1);
+	return -1.f / (jmj0 + jmj1 + jmj2 + jmj3);
 }
 
-
-void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA,
-	b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB, 
-	__global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,
-	b3ContactConstraint4_t* dstC )
+void setConstraint4(b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA,
+					b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB,
+					__global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,
+					b3ContactConstraint4_t* dstC)
 {
 	dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);
 	dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);
 
-	float dtInv = 1.f/dt;
-	for(int ic=0; ic<4; ic++)
+	float dtInv = 1.f / dt;
+	for (int ic = 0; ic < 4; ic++)
 	{
 		dstC->m_appliedRambdaDt[ic] = 0.f;
 	}
 	dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;
 
-
 	dstC->m_linear = src->m_worldNormalOnB;
-	dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );
-	for(int ic=0; ic<4; ic++)
+	dstC->m_linear.w = 0.7f;  //src->getFrictionCoeff() );
+	for (int ic = 0; ic < 4; ic++)
 	{
 		b3Float4 r0 = src->m_worldPosB[ic] - posA;
 		b3Float4 r1 = src->m_worldPosB[ic] - posB;
 
-		if( ic >= src->m_worldNormalOnB.w )//npoints
+		if (ic >= src->m_worldNormalOnB.w)  //npoints
 		{
 			dstC->m_jacCoeffInv[ic] = 0.f;
 			continue;
@@ -98,56 +93,56 @@ void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4Co
 			setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);
 
 			dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,
-				invMassA, &invInertiaA, invMassB, &invInertiaB );
+												   invMassA, &invInertiaA, invMassB, &invInertiaB);
 
 			relVelN = calcRelVel(linear, -linear, angular0, angular1,
-				linVelA, angVelA, linVelB, angVelB);
+								 linVelA, angVelA, linVelB, angVelB);
 
-			float e = 0.f;//src->getRestituitionCoeff();
-			if( relVelN*relVelN < 0.004f ) e = 0.f;
+			float e = 0.f;  //src->getRestituitionCoeff();
+			if (relVelN * relVelN < 0.004f) e = 0.f;
 
-			dstC->m_b[ic] = e*relVelN;
+			dstC->m_b[ic] = e * relVelN;
 			//float penetration = src->m_worldPosB[ic].w;
-			dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv;
+			dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift) * positionConstraintCoeff * dtInv;
 			dstC->m_appliedRambdaDt[ic] = 0.f;
 		}
 	}
 
-	if( src->m_worldNormalOnB.w > 0 )//npoints
-	{	//	prepare friction
-		b3Float4 center = b3MakeFloat4(0.f,0.f,0.f,0.f);
-		for(int i=0; i<src->m_worldNormalOnB.w; i++) 
+	if (src->m_worldNormalOnB.w > 0)  //npoints
+	{                                 //	prepare friction
+		b3Float4 center = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
+		for (int i = 0; i < src->m_worldNormalOnB.w; i++)
 			center += src->m_worldPosB[i];
 		center /= (float)src->m_worldNormalOnB.w;
 
 		b3Float4 tangent[2];
-		b3PlaneSpace1(src->m_worldNormalOnB,&tangent[0],&tangent[1]);
-		
+		b3PlaneSpace1(src->m_worldNormalOnB, &tangent[0], &tangent[1]);
+
 		b3Float4 r[2];
 		r[0] = center - posA;
 		r[1] = center - posB;
 
-		for(int i=0; i<2; i++)
+		for (int i = 0; i < 2; i++)
 		{
 			b3Float4 linear, angular0, angular1;
 			setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);
 
 			dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,
-				invMassA, &invInertiaA, invMassB, &invInertiaB );
+												   invMassA, &invInertiaA, invMassB, &invInertiaB);
 			dstC->m_fAppliedRambdaDt[i] = 0.f;
 		}
 		dstC->m_center = center;
 	}
 
-	for(int i=0; i<4; i++)
+	for (int i = 0; i < 4; i++)
 	{
-		if( i<src->m_worldNormalOnB.w )
+		if (i < src->m_worldNormalOnB.w)
 		{
 			dstC->m_worldPos[i] = src->m_worldPosB[i];
 		}
 		else
 		{
-			dstC->m_worldPos[i] = b3MakeFloat4(0.f,0.f,0.f,0.f);
+			dstC->m_worldPos[i] = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
 		}
 	}
 }
diff --git a/thirdparty/bullet/Bullet3Dynamics/shared/b3Inertia.h b/thirdparty/bullet/Bullet3Dynamics/shared/b3Inertia.h
index 96fe9f8b39..602a1335aa 100644
--- a/thirdparty/bullet/Bullet3Dynamics/shared/b3Inertia.h
+++ b/thirdparty/bullet/Bullet3Dynamics/shared/b3Inertia.h
@@ -11,5 +11,4 @@ struct b3Inertia
 	b3Mat3x3 m_initInvInertia;
 };
 
-
-#endif //B3_INERTIA_H
\ No newline at end of file
+#endif  //B3_INERTIA_H
\ No newline at end of file
diff --git a/thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h b/thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h
index e96f90d3f3..56d9118f95 100644
--- a/thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h
+++ b/thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h
@@ -2,11 +2,8 @@
 
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
 
-
-
-inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
+inline void integrateSingleTransform(__global b3RigidBodyData_t* bodies, int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
 {
-	
 	if (bodies[nodeID].m_invMass != 0.f)
 	{
 		float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
@@ -18,27 +15,27 @@ inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nod
 			bodies[nodeID].m_angVel.x *= angularDamping;
 			bodies[nodeID].m_angVel.y *= angularDamping;
 			bodies[nodeID].m_angVel.z *= angularDamping;
-			
+
 			b3Float4 angvel = bodies[nodeID].m_angVel;
 
 			float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
-			
+
 			//limit the angular motion
-			if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
+			if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
 			{
 				fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
 			}
-			if(fAngle < 0.001f)
+			if (fAngle < 0.001f)
 			{
 				// use Taylor's expansions of sync function
-				axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);
+				axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle);
 			}
 			else
 			{
 				// sync(fAngle) = sin(c*fAngle)/t
-				axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);
+				axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle);
 			}
-			
+
 			b3Quat dorn;
 			dorn.x = axis.x;
 			dorn.y = axis.y;
@@ -47,23 +44,21 @@ inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nod
 			b3Quat orn0 = bodies[nodeID].m_quat;
 			b3Quat predictedOrn = b3QuatMul(dorn, orn0);
 			predictedOrn = b3QuatNormalized(predictedOrn);
-			bodies[nodeID].m_quat=predictedOrn;
+			bodies[nodeID].m_quat = predictedOrn;
 		}
-		//linear velocity		
-		bodies[nodeID].m_pos +=  bodies[nodeID].m_linVel * timeStep;
-		
+		//linear velocity
+		bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep;
+
 		//apply gravity
 		bodies[nodeID].m_linVel += gravityAcceleration * timeStep;
-		
 	}
-	
 }
 
-inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
+inline void b3IntegrateTransform(__global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
 {
 	float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
-	
-	if( (body->m_invMass != 0.f))
+
+	if ((body->m_invMass != 0.f))
 	{
 		//angular velocity
 		{
@@ -72,23 +67,23 @@ inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeSt
 			body->m_angVel.x *= angularDamping;
 			body->m_angVel.y *= angularDamping;
 			body->m_angVel.z *= angularDamping;
-			
+
 			b3Float4 angvel = body->m_angVel;
 			float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
 			//limit the angular motion
-			if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
+			if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
 			{
 				fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
 			}
-			if(fAngle < 0.001f)
+			if (fAngle < 0.001f)
 			{
 				// use Taylor's expansions of sync function
-				axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);
+				axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle);
 			}
 			else
 			{
 				// sync(fAngle) = sin(c*fAngle)/t
-				axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);
+				axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle);
 			}
 			b3Quat dorn;
 			dorn.x = axis.x;
@@ -99,15 +94,13 @@ inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeSt
 
 			b3Quat predictedOrn = b3QuatMul(dorn, orn0);
 			predictedOrn = b3QuatNormalized(predictedOrn);
-			body->m_quat=predictedOrn;
+			body->m_quat = predictedOrn;
 		}
 
 		//apply gravity
 		body->m_linVel += gravityAcceleration * timeStep;
 
-		//linear velocity		
-		body->m_pos +=  body->m_linVel * timeStep;
-		
+		//linear velocity
+		body->m_pos += body->m_linVel * timeStep;
 	}
-	
 }