bullet: Streamline bundling, remove extraneous src/ folder

Document version and how to extract sources in thirdparty/README.md.
Drop unnecessary CMake and Premake files.
Simplify SCsub, drop unused one.

14 files changed, 5172 insertions, 0 deletions

diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h
new file mode 100644
index 0000000000..7a12257b33
--- /dev/null
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h
@@ -0,0 +1,159 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_CONTACT_SOLVER_INFO
+#define B3_CONTACT_SOLVER_INFO
+
+#include "Bullet3Common/b3Scalar.h"
+
+enum	b3SolverMode
+{
+	B3_SOLVER_RANDMIZE_ORDER = 1,
+	B3_SOLVER_FRICTION_SEPARATE = 2,
+	B3_SOLVER_USE_WARMSTARTING = 4,
+	B3_SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
+	B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
+	B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
+	B3_SOLVER_CACHE_FRIENDLY = 128,
+	B3_SOLVER_SIMD = 256,
+	B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
+	B3_SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024
+};
+
+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;
+
+
+};
+
+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_restitution = b3Scalar(0.);
+		m_maxErrorReduction = b3Scalar(20.);
+		m_numIterations = 10;
+		m_erp = b3Scalar(0.2);
+		m_erp2 = b3Scalar(0.8);
+		m_globalCfm = b3Scalar(0.);
+		m_sor = b3Scalar(1.);
+		m_splitImpulse = true;
+		m_splitImpulsePenetrationThreshold = -.04f;
+		m_splitImpulseTurnErp = 0.1f;
+		m_linearSlop = b3Scalar(0.0);
+		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.
+	}
+};
+
+///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];
+
+};
+///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];
+};
+
+
+
+#endif //B3_CONTACT_SOLVER_INFO
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp
new file mode 100644
index 0000000000..5e11e74935
--- /dev/null
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp
@@ -0,0 +1,108 @@
+
+#include "b3FixedConstraint.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#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)
+{
+	m_pivotInA = frameInA.getOrigin();
+	m_pivotInB = frameInB.getOrigin();
+	m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
+
+}
+
+b3FixedConstraint::~b3FixedConstraint ()
+{
+}
+
+	
+void b3FixedConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
+{
+	info->m_numConstraintRows = 6;
+	info->nub = 6;
+}
+
+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 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;
+
+	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 a1neg = -a1;
+		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;
+	}
+	
+	b3Vector3 a2 = b3QuatRotate(worldOrnB,m_pivotInB);
+   
+	{
+	//	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);
+	}
+
+    // 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];
+		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
+    }
+
+		//fix the 3 angular degrees of freedom
+
+	int start_row = 3;
+	int s = info->rowskip;
+    int start_index = start_row * s;
+
+    // 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;
+    }
+
+
+    // 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];
+    }
+
+}
\ No newline at end of file
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h
new file mode 100644
index 0000000000..e884a82912
--- /dev/null
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h
@@ -0,0 +1,35 @@
+
+#ifndef B3_FIXED_CONSTRAINT_H
+#define B3_FIXED_CONSTRAINT_H
+
+#include "b3TypedConstraint.h"
+
+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);
+	
+	virtual ~b3FixedConstraint();
+
+	
+	virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
+
+	virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies);
+
+	virtual	void	setParam(int num, b3Scalar value, int axis = -1)
+	{
+		b3Assert(0);
+	}
+	virtual	b3Scalar getParam(int num, int axis = -1) const
+	{
+		b3Assert(0);
+		return 0.f;
+	}
+
+};
+
+#endif //B3_FIXED_CONSTRAINT_H
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp
new file mode 100644
index 0000000000..168a773d56
--- /dev/null
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp
@@ -0,0 +1,807 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+/*
+2007-09-09
+Refactored by Francisco Le?n
+email: projectileman@yahoo.com
+http://gimpact.sf.net
+*/
+
+#include "b3Generic6DofConstraint.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+
+#include "Bullet3Common/b3TransformUtil.h"
+#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)
+{
+	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;
+	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)
+{
+	//	// 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);
+	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));
+			return true;
+		}
+		else
+		{
+			// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
+			xyz[0] = -b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+			xyz[1] = -B3_HALF_PI;
+			xyz[2] = b3Scalar(0.0);
+			return false;
+		}
+	}
+	else
+	{
+		// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
+		xyz[0] = b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+		xyz[1] = B3_HALF_PI;
+		xyz[2] = 0.0;
+	}
+	return false;
+}
+
+//////////////////////////// b3RotationalLimitMotor ////////////////////////////////////
+
+int b3RotationalLimitMotor::testLimitValue(b3Scalar test_value)
+{
+	if(m_loLimit>m_hiLimit)
+	{
+		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;
+		return 1;
+	}
+	else if (test_value> m_hiLimit)
+	{
+		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;
+		return 2;
+	};
+
+	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)
+	{
+		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;
+		return 2;
+	}
+	else if (test_value> hiLimit)
+	{
+		m_currentLimit[limitIndex] = 1;//High limit violation
+		m_currentLimitError[limitIndex] = test_value - hiLimit;
+		return 1;
+	};
+
+	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);
+	// 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]) :
+	//
+	//    to get			constrain w2-w1 along		...not
+	//    ------			---------------------		------
+	//    d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
+	//    d(angle[1])/dt = 0	ax[1]
+	//    d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
+	//
+	// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
+	// to prove the result for angle[0], write the expression for angle[0] from
+	// GetInfo1 then take the derivative. to prove this for angle[2] it is
+	// easier to take the euler rate expression for d(angle[2])/dt with respect
+	// to the components of w and set that to 0.
+	b3Vector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
+	b3Vector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
+
+	m_calculatedAxis[1] = axis2.cross(axis0);
+	m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
+	m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
+
+	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);
+	return tr;
+}
+
+void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyData* bodies)
+{
+	b3Transform transA;
+	b3Transform transB;
+	transA = getCenterOfMassTransform(bodies[m_rbA]);
+	transB = getCenterOfMassTransform(bodies[m_rbB]);
+	calculateTransforms(transA,transB,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
+		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))
+		{
+			m_factA = miB / miS;
+		}
+		else 
+		{
+			m_factA = b3Scalar(0.5f);
+		}
+		m_factB = b3Scalar(1.0f) - m_factA;
+	}
+}
+
+
+
+
+
+
+
+bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
+{
+	b3Scalar angle = m_calculatedAxisAngleDiff[axis_index];
+	angle = b3AdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
+	m_angularLimits[axis_index].m_currentPosition = angle;
+	//test limits
+	m_angularLimits[axis_index].testLimitValue(angle);
+	return m_angularLimits[axis_index].needApplyTorques();
+}
+
+
+
+
+void b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
+{
+	//prepare constraint
+	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++)
+	{
+		if(m_linearLimits.needApplyForce(i))
+		{
+			info->m_numConstraintRows++;
+			info->nub--;
+		}
+	}
+	//test angular limits
+	for (i=0;i<3 ;i++ )
+	{
+		if(testAngularLimitMotor(i))
+		{
+			info->m_numConstraintRows++;
+			info->nub--;
+		}
+	}
+//	printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows);
+}
+
+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)
+{
+
+	b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]);
+	b3Transform transB = getCenterOfMassTransform(bodies[m_rbB]);
+	const b3Vector3& linVelA = bodies[m_rbA].m_linVel;
+	const b3Vector3& linVelB = bodies[m_rbB].m_linVel;
+	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);
+	}
+	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);
+	}
+
+}
+
+
+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);
+
+	int 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);
+	}
+	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);
+	}
+}
+
+
+
+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;
+	//solve linear limits
+	b3RotationalLimitMotor limot;
+	for (int i=0;i<3 ;i++ )
+	{
+		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];
+			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)
+			{
+				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)
+				{
+					rotAllowed = 0;
+				}
+				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);
+			}
+		}
+	}
+	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)
+{
+	b3Generic6DofConstraint * d6constraint = this;
+	int row = row_offset;
+	//solve angular limits
+	for (int i=0;i<3 ;i++ )
+	{
+		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))
+			{
+				m_angularLimits[i].m_normalCFM = info->cfm[0];
+			}
+			if(!(flags & B3_6DOF_FLAGS_CFM_STOP))
+			{
+				m_angularLimits[i].m_stopCFM = info->cfm[0];
+			}
+			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);
+		}
+	}
+
+	return row;
+}
+
+
+
+
+void	b3Generic6DofConstraint::updateRHS(b3Scalar	timeStep)
+{
+	(void)timeStep;
+
+}
+
+
+void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB,const b3RigidBodyData* bodies)
+{
+	m_frameInA = frameA;
+	m_frameInB = frameB;
+
+	calculateTransforms(bodies);
+}
+
+
+
+b3Vector3 b3Generic6DofConstraint::getAxis(int axis_index) const
+{
+	return m_calculatedAxis[axis_index];
+}
+
+
+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))
+	{
+		weight = b3Scalar(1.0);
+	}
+	else
+	{
+		weight = imA / (imA + imB);
+	}
+	const b3Vector3& pA = m_calculatedTransformA.getOrigin();
+	const b3Vector3& pB = m_calculatedTransformB.getOrigin();
+	m_AnchorPos = pA * weight + pB * (b3Scalar(1.0) - weight);
+	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++)
+	{
+		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)
+{
+    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];
+		}
+		if (J2)
+		{
+			J2[srow+0] = -ax1[0];
+			J2[srow+1] = -ax1[1];
+			J2[srow+2] = -ax1[2];
+		}
+		if((!rotational))
+        {
+			if (m_useOffsetForConstraintFrame)
+			{
+				b3Vector3 tmpA, tmpB, relA, relB;
+				// get vector from bodyB to frameB in WCS
+				relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
+				// get its projection to constraint axis
+				b3Vector3 projB = ax1 * relB.dot(ax1);
+				// get vector directed from bodyB to constraint axis (and orthogonal to it)
+				b3Vector3 orthoB = relB - projB;
+				// same for bodyA
+				relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
+				b3Vector3 projA = ax1 * relA.dot(ax1);
+				b3Vector3 orthoA = relA - projA;
+				// get desired offset between frames A and B along constraint axis
+				b3Scalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
+				// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
+				b3Vector3 totalDist = projA + ax1 * desiredOffs - projB;
+				// get offset vectors relA and relB
+				relA = orthoA + totalDist * m_factA;
+				relB = orthoB - totalDist * m_factB;
+				tmpA = relA.cross(ax1);
+				tmpB = relB.cross(ax1);
+				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
+			{
+				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];
+
+				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];
+			}
+        }
+        // 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)
+            {
+				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);
+				info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
+                info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
+                info->m_upperLimit[srow] = limot->m_maxMotorForce;
+            }
+        }
+        if(limit)
+        {
+            b3Scalar k = info->fps * limot->m_stopERP;
+			if(!rotational)
+			{
+				info->m_constraintError[srow] += k * limot->m_currentLimitError;
+			}
+			else
+			{
+				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]) 
+								info->m_constraintError[srow] = newc;
+                        }
+                    }
+                    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;
+}
+
+
+
+
+
+
+	///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))
+	{
+		switch(num)
+		{
+			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 : 
+				m_linearLimits.m_stopCFM[axis] = value;
+				m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
+				break;
+			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 : 
+				b3AssertConstrParams(0);
+		}
+	}
+	else if((axis >=3) && (axis < 6))
+	{
+		switch(num)
+		{
+			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 : 
+				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 : 
+				m_angularLimits[axis - 3].m_normalCFM = value;
+				m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
+				break;
+			default : 
+				b3AssertConstrParams(0);
+		}
+	}
+	else
+	{
+		b3AssertConstrParams(0);
+	}
+}
+
+	///return the local value of parameter
+b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const 
+{
+	b3Scalar retVal = 0;
+	if((axis >= 0) && (axis < 3))
+	{
+		switch(num)
+		{
+			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 : 
+				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 : 
+				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
+				retVal = m_linearLimits.m_normalCFM[axis];
+				break;
+			default : 
+				b3AssertConstrParams(0);
+		}
+	}
+	else if((axis >=3) && (axis < 6))
+	{
+		switch(num)
+		{
+			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 : 
+				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 : 
+				b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
+				retVal = m_angularLimits[axis - 3].m_normalCFM;
+				break;
+			default : 
+				b3AssertConstrParams(0);
+		}
+	}
+	else
+	{
+		b3AssertConstrParams(0);
+	}
+	return retVal;
+}
+
+ 
+
+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
+	
+	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]);
+	
+	// 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
new file mode 100644
index 0000000000..084d36055c
--- /dev/null
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h
@@ -0,0 +1,550 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+/// 2009 March: b3Generic6DofConstraint refactored by Roman Ponomarev
+/// Added support for generic constraint solver through getInfo1/getInfo2 methods
+
+/*
+2007-09-09
+b3Generic6DofConstraint Refactored by Francisco Le?n
+email: projectileman@yahoo.com
+http://gimpact.sf.net
+*/
+
+
+#ifndef B3_GENERIC_6DOF_CONSTRAINT_H
+#define B3_GENERIC_6DOF_CONSTRAINT_H
+
+#include "Bullet3Common/b3Vector3.h"
+#include "b3JacobianEntry.h"
+#include "b3TypedConstraint.h"
+
+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 = 0.1f;
+        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_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;
+    }
+
+
+
+	//! Is limited
+    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;
+    }
+
+	//! calculates  error
+	/*!
+	calculates m_currentLimit and m_currentLimitError.
+	*/
+	int testLimitValue(b3Scalar test_value);
+
+	//! apply the correction impulses for two bodies
+    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
+	//!@}
+	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_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;
+
+    	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++) 
+		{
+			m_enableMotor[i] = other.m_enableMotor[i];
+			m_targetVelocity[i] = other.m_targetVelocity[i];
+			m_maxMotorForce[i] = other.m_maxMotorForce[i];
+		}
+    }
+
+    //! 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;
+    }
+	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);
+
+
+};
+
+enum b36DofFlags
+{
+	B3_6DOF_FLAGS_CFM_NORM = 1,
+	B3_6DOF_FLAGS_CFM_STOP = 2,
+	B3_6DOF_FLAGS_ERP_STOP = 4
+};
+#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
+/*!
+b3Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
+currently this limit supports rotational motors<br>
+<ul>
+<li> For Linear limits, use b3Generic6DofConstraint.setLinearUpperLimit, b3Generic6DofConstraint.setLinearLowerLimit. You can set the parameters with the b3TranslationalLimitMotor structure accsesible through the b3Generic6DofConstraint.getTranslationalLimitMotor method.
+At this moment translational motors are not supported. May be in the future. </li>
+
+<li> For Angular limits, use the b3RotationalLimitMotor structure for configuring the limit.
+This is accessible through b3Generic6DofConstraint.getLimitMotor method,
+This brings support for limit parameters and motors. </li>
+
+<li> Angulars limits have these possible ranges:
+<table border=1 >
+<tr>
+	<td><b>AXIS</b></td>
+	<td><b>MIN ANGLE</b></td>
+	<td><b>MAX ANGLE</b></td>
+</tr><tr>
+	<td>X</td>
+	<td>-PI</td>
+	<td>PI</td>
+</tr><tr>
+	<td>Y</td>
+	<td>-PI/2</td>
+	<td>PI/2</td>
+</tr><tr>
+	<td>Z</td>
+	<td>-PI</td>
+	<td>PI</td>
+</tr>
+</table>
+</li>
+</ul>
+
+*/
+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
+    //!@}
+
+    //! 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];
+	//!@}
+
+
+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
+
+    bool	m_useLinearReferenceFrameA;
+	bool	m_useOffsetForConstraintFrame;
+    
+	int		m_flags;
+
+    //!@}
+
+    b3Generic6DofConstraint&	operator=(b3Generic6DofConstraint&	other)
+    {
+        b3Assert(0);
+        (void) other;
+        return *this;
+    }
+
+
+	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();
+
+
+
+public:
+
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+	
+    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 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;
+    }
+
+    //! 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;
+    }
+
+    b3Transform & getFrameOffsetB()
+    {
+    	return m_frameInB;
+    }
+
+
+
+	virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies);
+
+	void getInfo1NonVirtual (b3ConstraintInfo1* info,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);
+
+	//! Get the rotation axis in global coordinates
+    b3Vector3 getAxis(int axis_index) const;
+
+    //! Get the relative Euler angle
+    /*!
+	\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
+	*/
+    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);
+
+	//! 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);
+
+    void	setLinearLowerLimit(const b3Vector3& linearLower)
+    {
+    	m_linearLimits.m_lowerLimit = linearLower;
+    }
+
+	void	getLinearLowerLimit(b3Vector3& linearLower)
+	{
+		linearLower = m_linearLimits.m_lowerLimit;
+	}
+
+	void	setLinearUpperLimit(const b3Vector3& linearUpper)
+	{
+		m_linearLimits.m_upperLimit = linearUpper;
+	}
+
+	void	getLinearUpperLimit(b3Vector3& linearUpper)
+	{
+		linearUpper = m_linearLimits.m_upperLimit;
+	}
+
+    void	setAngularLowerLimit(const b3Vector3& angularLower)
+    {
+		for(int i = 0; i < 3; i++) 
+			m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]);
+    }
+
+	void	getAngularLowerLimit(b3Vector3& angularLower)
+	{
+		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++)
+			m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]);
+    }
+
+	void	getAngularUpperLimit(b3Vector3& angularUpper)
+	{
+		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
+    	{
+			lo = b3NormalizeAngle(lo);
+			hi = b3NormalizeAngle(hi);
+    		m_angularLimits[axis-3].m_loLimit = lo;
+    		m_angularLimits[axis-3].m_hiLimit = hi;
+    	}
+    }
+
+	//! Test limit
+	/*!
+    - free means upper < lower,
+    - locked means upper == lower
+    - limited means upper > lower
+    - limitIndex: first 3 are linear, next 3 are angular
+    */
+    bool	isLimited(int limitIndex)
+    {
+    	if(limitIndex<3)
+    	{
+			return m_linearLimits.isLimited(limitIndex);
+
+    	}
+        return m_angularLimits[limitIndex-3].isLimited();
+    }
+
+	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);
+
+	// 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). 
+	///If no axis is provided, it uses the default axis for this constraint.
+	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);
+
+
+
+	
+};
+
+
+
+
+
+#endif //B3_GENERIC_6DOF_CONSTRAINT_H
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h
new file mode 100644
index 0000000000..a55168eb38
--- /dev/null
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h
@@ -0,0 +1,155 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_JACOBIAN_ENTRY_H
+#define B3_JACOBIAN_ENTRY_H
+
+#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
+// if you only are interested in angular part, just feed massInvA and massInvB zero
+
+/// 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
+{
+public:
+	b3JacobianEntry() {};
+	//constraint between two different rigidbodies
+	b3JacobianEntry(
+		const b3Matrix3x3& world2A,
+		const b3Matrix3x3& world2B,
+		const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
+		const b3Vector3& jointAxis,
+		const b3Vector3& inertiaInvA, 
+		const b3Scalar massInvA,
+		const b3Vector3& inertiaInvB,
+		const b3Scalar massInvB)
+		: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_1MinvJt = inertiaInvB * m_bJ;
+		m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
+
+		b3Assert(m_Adiag > b3Scalar(0.0));
+	}
+
+	//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.)))
+	{
+		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);
+
+		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)
+	{
+		m_0MinvJt	= inertiaInvA * m_aJ;
+		m_1MinvJt = inertiaInvB * m_bJ;
+		m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
+
+		b3Assert(m_Adiag > b3Scalar(0.0));
+	}
+
+	//constraint on one rigidbody
+	b3JacobianEntry(
+		const b3Matrix3x3& world2A,
+		const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
+		const b3Vector3& jointAxis,
+		const b3Vector3& inertiaInvA, 
+		const b3Scalar massInvA)
+		: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_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
+
+		b3Assert(m_Adiag > b3Scalar(0.0));
+	}
+
+	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
+	{
+		const b3JacobianEntry& jacA = *this;
+		b3Scalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
+		b3Scalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
+		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
+	{
+		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 lin1 = massInvB * lin;
+		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)
+	{
+		b3Vector3 linrel = linvelA - linvelB;
+		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];
+		return rel_vel2 + B3_EPSILON;
+	}
+//private:
+
+	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;
+
+};
+
+#endif //B3_JACOBIAN_ENTRY_H
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp
new file mode 100644
index 0000000000..de729d4556
--- /dev/null
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp
@@ -0,0 +1,1815 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2012 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+//enable B3_SOLVER_DEBUG if you experience solver crashes
+//#define B3_SOLVER_DEBUG
+//#define COMPUTE_IMPULSE_DENOM 1
+//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
+
+//#define DISABLE_JOINTS
+
+#include "b3PgsJacobiSolver.h"
+#include "Bullet3Common/b3MinMax.h"
+#include "b3TypedConstraint.h"
+#include <new>
+#include "Bullet3Common/b3StackAlloc.h"
+
+//#include "b3SolverBody.h"
+//#include "b3SolverConstraint.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#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)
+{
+	b3Transform newTrans;
+	newTrans.setOrigin(rb->m_pos);
+	newTrans.setRotation(rb->m_quat);
+	return newTrans;
+}
+
+static const b3Matrix3x3&	getInvInertiaTensorWorld(b3InertiaData* inertia)
+{
+	return inertia->m_invInertiaWorld;
+}
+
+
+
+static const b3Vector3&	getLinearVelocity(b3RigidBodyData* rb)
+{
+	return rb->m_linVel;
+}
+
+static const b3Vector3&	getAngularVelocity(b3RigidBodyData* rb)
+{
+	return rb->m_angVel;
+}
+
+static b3Vector3 getVelocityInLocalPoint(b3RigidBodyData* rb, const b3Vector3& rel_pos)
+{
+	//we also calculate lin/ang velocity for kinematic objects
+	return getLinearVelocity(rb) + getAngularVelocity(rb).cross(rel_pos);
+	
+}
+
+struct	b3ContactPoint
+{
+	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()
+	{
+		return m_positionWorldOnA;
+	}
+	b3Vector3	getPositionWorldOnB()
+	{
+		return m_positionWorldOnB;
+	}
+	b3Scalar	getDistance()
+	{
+		return m_distance;
+	}
+};
+
+void	getContactPoint(b3Contact4* contact, int contactIndex, b3ContactPoint& pointOut)
+{
+	pointOut.m_appliedImpulse = 0.f;
+	pointOut.m_appliedImpulseLateral1 = 0.f;
+	pointOut.m_appliedImpulseLateral2 = 0.f;
+	pointOut.m_combinedFriction = contact->getFrictionCoeff();
+	pointOut.m_combinedRestitution = contact->getRestituitionCoeff();
+	pointOut.m_combinedRollingFriction = 0.f;
+	pointOut.m_contactCFM1 = 0.f;
+	pointOut.m_contactCFM2 = 0.f;
+	pointOut.m_contactMotion1 = 0.f;
+	pointOut.m_contactMotion2 = 0.f;
+	pointOut.m_distance = contact->getPenetration(contactIndex);//??0.01f
+	b3Vector3 normalOnB = contact->m_worldNormalOnB;
+	normalOnB.normalize();//is this needed?
+
+	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;
+}
+
+int	getNumContacts(b3Contact4* contact)
+{
+	return contact->getNPoints();
+}
+
+b3PgsJacobiSolver::b3PgsJacobiSolver(bool usePgs)
+: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)
+{
+	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_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS;
+	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);
+
+	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)
+{
+
+	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;
+}
+
+
+
+
+
+
+
+
+
+#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 )
+{
+	__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
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+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 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));
+#else
+	resolveSingleConstraintRowGeneric(body1,body2,c);
+#endif
+}
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+ 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());
+
+//	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;
+		c.m_appliedImpulse = c.m_lowerLimit;
+	}
+	else if (sum > c.m_upperLimit) 
+	{
+		deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
+		c.m_appliedImpulse = c.m_upperLimit;
+	}
+	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);
+}
+
+ 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 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));
+#else
+	resolveSingleConstraintRowLowerLimit(body1,body2,c);
+#endif
+}
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+ 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());
+
+	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;
+		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);
+}
+
+
+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);
+        }
+}
+
+ void b3PgsJacobiSolver::resolveSplitPenetrationSIMD(b3SolverBody& body1,b3SolverBody& body2,const b3SolverConstraint& c)
+{
+#ifdef USE_SIMD
+	if (!c.m_rhsPenetration)
+		return;
+
+	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 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));
+#else
+	resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
+#endif
+}
+
+
+
+unsigned long b3PgsJacobiSolver::b3Rand2()
+{
+	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)
+{
+	// seems good; xor-fold and modulus
+	const unsigned long un = static_cast<unsigned long>(n);
+	unsigned long r = b3Rand2();
+
+	// 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) {
+		r ^= (r >> 16);
+		if (un <= 0x00000100UL) {
+			r ^= (r >> 8);
+			if (un <= 0x00000010UL) {
+				r ^= (r >> 4);
+				if (un <= 0x00000004UL) {
+					r ^= (r >> 2);
+					if (un <= 0x00000002UL) {
+						r ^= (r >> 1);
+					}
+				}
+			}
+		}
+	}
+
+	return (int) (r % un);
+}
+
+
+
+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);
+
+	if (rb)
+	{
+		solverBody->m_worldTransform = getWorldTransform(rb);
+		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_linearVelocity = getLinearVelocity(rb);
+		solverBody->m_angularVelocity = getAngularVelocity(rb);
+	} else
+	{
+		solverBody->m_worldTransform.setIdentity();
+		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);
+	}
+
+
+}
+
+
+
+
+
+
+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)
+{
+
+	
+	solverConstraint.m_contactNormal = normalAxis;
+	b3SolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
+	b3SolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
+
+	b3RigidBodyData* body0 = &bodies[solverBodyA.m_originalBodyIndex];
+	b3RigidBodyData* body1 = &bodies[solverBodyB.m_originalBodyIndex];
+
+
+	solverConstraint.m_solverBodyIdA = solverBodyIdA;
+	solverConstraint.m_solverBodyIdB = solverBodyIdB;
+
+	solverConstraint.m_friction = cp.m_combinedFriction;
+	solverConstraint.m_originalContactPoint = 0;
+
+	solverConstraint.m_appliedImpulse = 0.f;
+	solverConstraint.m_appliedPushImpulse = 0.f;
+
+	{
+		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);
+	}
+	{
+		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);
+	}
+
+	b3Scalar scaledDenom;
+
+	{
+		b3Vector3 vec;
+		b3Scalar denom0 = 0.f;
+		b3Scalar denom1 = 0.f;
+		if (body0)
+		{
+			vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+			denom0 = body0->m_invMass + normalAxis.dot(vec);
+		}
+		if (body1)
+		{
+			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
+		{
+			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);
+		}
+
+		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));
+
+		rel_vel = vel1Dotn+vel2Dotn;
+
+//		b3Scalar positionalError = 0.f;
+
+		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& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
+	solverConstraint.m_frictionIndex = frictionIndex;
+	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)
+
+{
+	b3Vector3 normalAxis=b3MakeVector3(0,0,0);
+
+
+	solverConstraint.m_contactNormal = normalAxis;
+	b3SolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
+	b3SolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
+
+	b3RigidBodyData* body0 = &bodies[m_tmpSolverBodyPool[solverBodyIdA].m_originalBodyIndex];
+	b3RigidBodyData* body1 = &bodies[m_tmpSolverBodyPool[solverBodyIdB].m_originalBodyIndex];
+
+	solverConstraint.m_solverBodyIdA = solverBodyIdA;
+	solverConstraint.m_solverBodyIdB = solverBodyIdB;
+
+	solverConstraint.m_friction = cp.m_combinedRollingFriction;
+	solverConstraint.m_originalContactPoint = 0;
+
+	solverConstraint.m_appliedImpulse = 0.f;
+	solverConstraint.m_appliedPushImpulse = 0.f;
+
+	{
+		b3Vector3 ftorqueAxis1 = -normalAxis1;
+		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
+		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);
+	}
+
+
+	{
+		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;
+	}
+
+	{
+		
+
+		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));
+
+		rel_vel = vel1Dotn+vel2Dotn;
+
+//		b3Scalar positionalError = 0.f;
+
+		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& 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);
+	return solverConstraint;
+}
+
+
+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_bodyCount[bodyIndex]<0)
+		{
+			curIndex = m_tmpSolverBodyPool.size();
+			b3SolverBody& solverBody = m_tmpSolverBodyPool.expand();
+			initSolverBody(bodyIndex,&solverBody,&body);
+			solverBody.m_originalBodyIndex = bodyIndex;
+			m_bodyCount[bodyIndex] = curIndex;
+		} else
+		{
+			curIndex = m_bodyCount[bodyIndex];
+		}
+	} else
+	{
+		b3Assert(m_bodyCount[bodyIndex]>0);
+		m_bodyCountCheck[bodyIndex]++;
+		curIndex = m_tmpSolverBodyPool.size();
+		b3SolverBody& solverBody = m_tmpSolverBodyPool.expand();
+		initSolverBody(bodyIndex,&solverBody,&body);
+		solverBody.m_originalBodyIndex = bodyIndex;
+	}
+
+	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)
+{
+			
+			const b3Vector3& pos1 = cp.getPositionWorldOnA();
+			const b3Vector3& pos2 = cp.getPositionWorldOnB();
+
+			b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
+			b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
+
+			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();
+
+			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);
+
+			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;
+
+					vel1 = rb0? getVelocityInLocalPoint(rb0,rel_pos1) : b3MakeVector3(0,0,0);
+					vel2 = rb1? getVelocityInLocalPoint(rb1, rel_pos2) : b3MakeVector3(0,0,0);
+
+	//			b3Vector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : b3Vector3(0,0,0);
+					vel  = vel1 - vel2;
+					rel_vel = cp.m_normalWorldOnB.dot(vel);
+
+					
+
+					solverConstraint.m_friction = cp.m_combinedFriction;
+
+				
+					restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
+					if (restitution <= b3Scalar(0.))
+					{
+						restitution = 0.f;
+					};
+				}
+
+
+				///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;
+				}
+
+				solverConstraint.m_appliedPushImpulse = 0.f;
+
+				{
+					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;
+					}
+
+					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)
+{
+
+	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);
+			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
+		{
+			frictionConstraint1.m_appliedImpulse = 0.f;
+		}
+	}
+
+	if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
+	{
+		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;
+			if (bodies[bodyA->m_originalBodyIndex].m_invMass)
+				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
+		{
+			frictionConstraint2.m_appliedImpulse = 0.f;
+		}
+	}
+}
+
+
+
+
+void	b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal)
+{
+	b3RigidBodyData* colObj0=0,*colObj1=0;
+
+	
+	int solverBodyIdA = getOrInitSolverBody(manifold->getBodyA(),bodies,inertias);
+	int solverBodyIdB = getOrInitSolverBody(manifold->getBodyB(),bodies,inertias);
+
+//	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 numContacts = getNumContacts(manifold);
+	for (int j=0;j<numContacts;j++)
+	{
+
+		b3ContactPoint cp;
+		getContactPoint(manifold,j,cp);
+
+		if (cp.getDistance() <= getContactProcessingThreshold(manifold))
+		{
+			b3Vector3 rel_pos1;
+			b3Vector3 rel_pos2;
+			b3Scalar relaxation;
+			b3Scalar rel_vel;
+			b3Vector3 vel;
+
+			int frictionIndex = m_tmpSolverContactConstraintPool.size();
+			b3SolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
+//			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);
+
+//			const b3Vector3& pos1 = cp.getPositionWorldOnA();
+//			const b3Vector3& pos2 = cp.getPositionWorldOnB();
+
+			/////setup the friction constraints
+
+			solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
+
+			b3Vector3 angVelA,angVelB;
+			solverBodyA->getAngularVelocity(angVelA);
+			solverBodyB->getAngularVelocity(angVelB);			
+			b3Vector3 relAngVel = angVelB-angVelA;
+
+			if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
+			{
+				//only a single rollingFriction per manifold
+				rollingFriction--;
+				if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
+				{
+					relAngVel.normalize();
+					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);
+		
+				}
+			}
+
+			///Bullet has several options to set the friction directions
+			///By default, each contact has only a single friction direction that is recomputed automatically very frame 
+			///based on the relative linear velocity.
+			///If the relative velocity it zero, it will automatically compute a friction direction.
+			
+			///You can also enable two friction directions, using the 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, 
+			///and set the cp.m_lateralFrictionInitialized to true
+			///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
+			///this will give a conveyor belt effect
+			///
+			if (!(infoGlobal.m_solverMode & B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+			{
+				cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+				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_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);
+
+					}
+
+					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);
+
+					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_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
+			{
+				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);
+
+				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)
+{
+	B3_PROFILE("solveGroupCacheFriendlySetup");
+
+
+	m_maxOverrideNumSolverIterations = 0;
+
+
+
+	m_tmpSolverBodyPool.resize(0);
+	
+	
+	m_bodyCount.resize(0);
+	m_bodyCount.resize(numBodies,0);
+	m_bodyCountCheck.resize(0);
+	m_bodyCountCheck.resize(numBodies,0);
+	
+	m_deltaLinearVelocities.resize(0);
+	m_deltaLinearVelocities.resize(numBodies,b3MakeVector3(0,0,0));
+	m_deltaAngularVelocities.resize(0);
+	m_deltaAngularVelocities.resize(numBodies,b3MakeVector3(0,0,0));
+	
+	//int totalBodies = 0;
+
+	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
+		{
+			//didn't implement joints with Jacobi version yet
+			b3Assert(0);
+		}
+
+	}
+	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
+		{
+			if (bodies[bodyIndexA].m_invMass)
+			{
+				//m_bodyCount[bodyIndexA]+=manifoldPtr[i].getNPoints();
+				m_bodyCount[bodyIndexA]++;
+			}
+			else
+				m_bodyCount[bodyIndexA]=-1;
+
+			if (bodies[bodyIndexB].m_invMass)
+			//	m_bodyCount[bodyIndexB]+=manifoldPtr[i].getNPoints();
+				m_bodyCount[bodyIndexB]++;
+			else
+				m_bodyCount[bodyIndexB]=-1;
+		}
+
+	}
+
+
+	
+	if (1)
+	{
+		int j;
+		for (j=0;j<numConstraints;j++)
+		{
+			b3TypedConstraint* constraint = constraints[j];
+			
+			constraint->internalSetAppliedImpulse(0.0f);
+		}
+	}
+
+	//b3RigidBody* rb0=0,*rb1=0;
+	//if (1)
+	{
+		{
+
+			int totalNumRows = 0;
+			int i;
+			
+			m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
+			//calculate the total number of contraint rows
+			for (i=0;i<numConstraints;i++)
+			{
+				b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+				b3JointFeedback* fb = constraints[i]->getJointFeedback();
+				if (fb)
+				{
+					fb->m_appliedForceBodyA.setZero();
+					fb->m_appliedTorqueBodyA.setZero();
+					fb->m_appliedForceBodyB.setZero();
+					fb->m_appliedTorqueBodyB.setZero();
+				}
+
+				if (constraints[i]->isEnabled())
+				{
+				}
+				if (constraints[i]->isEnabled())
+				{
+					constraints[i]->getInfo1(&info1,bodies);
+				} else
+				{
+					info1.m_numConstraintRows = 0;
+					info1.nub = 0;
+				}
+				totalNumRows += info1.m_numConstraintRows;
+			}
+			m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
+
+			
+#ifndef DISABLE_JOINTS
+			///setup the b3SolverConstraints
+			int currentRow = 0;
+
+			for (i=0;i<numConstraints;i++)
+			{
+				const b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+				
+				if (info1.m_numConstraintRows)
+				{
+					b3Assert(currentRow<totalNumRows);
+
+					b3SolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
+					b3TypedConstraint* constraint = constraints[i];
+
+					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];
+
+
+
+
+					int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
+					if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
+						m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
+
+
+					int j;
+					for ( j=0;j<info1.m_numConstraintRows;j++)
+					{
+						memset(&currentConstraintRow[j],0,sizeof(b3SolverConstraint));
+						currentConstraintRow[j].m_lowerLimit = -B3_INFINITY;
+						currentConstraintRow[j].m_upperLimit = B3_INFINITY;
+						currentConstraintRow[j].m_appliedImpulse = 0.f;
+						currentConstraintRow[j].m_appliedPushImpulse = 0.f;
+						currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
+						currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
+						currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
+					}
+
+					bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
+					bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
+					bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
+					bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
+					bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
+					bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
+					bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
+					bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
+
+
+					b3TypedConstraint::b3ConstraintInfo2 info2;
+					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.m_constraintError = &currentConstraintRow->m_rhs;
+					currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
+					info2.m_damping = infoGlobal.m_damping;
+					info2.cfm = &currentConstraintRow->m_cfm;
+					info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
+					info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
+					info2.m_numIterations = infoGlobal.m_numIterations;
+					constraints[i]->getInfo2(&info2,bodies);
+
+					///finalize the constraint setup
+					for ( j=0;j<info1.m_numConstraintRows;j++)
+					{
+						b3SolverConstraint& solverConstraint = currentConstraintRow[j];
+
+						if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold())
+						{
+							solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
+						}
+
+						if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold())
+						{
+							solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
+						}
+
+						solverConstraint.m_originalContactPoint = constraint;
+							
+						b3Matrix3x3& invInertiaWorldA= inertias[constraint->getRigidBodyA()].m_invInertiaWorld;
+						{
+
+							//b3Vector3 angularFactorA(1,1,1);
+							const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
+							solverConstraint.m_angularComponentA = invInertiaWorldA*ftorqueAxis1;//*angularFactorA;
+						}
+						
+						b3Matrix3x3& invInertiaWorldB= inertias[constraint->getRigidBodyB()].m_invInertiaWorld;
+						{
+
+							const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
+							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;
+
+							b3Scalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
+							sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
+							sum += iMJlB.dot(solverConstraint.m_contactNormal);
+							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;
+						}
+
+
+						///fix rhs
+						///todo: add force/torque accelerators
+						{
+							b3Scalar rel_vel;
+							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;
+
+							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;
+							solverConstraint.m_appliedImpulse = 0.f;
+
+						}
+					}
+				}
+				currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
+			}
+#endif //DISABLE_JOINTS
+		}
+
+
+		{
+			int i;
+
+			for (i=0;i<numManifolds;i++)
+			{
+				b3Contact4& manifold = manifoldPtr[i];
+				convertContact(bodies,inertias,&manifold,infoGlobal);
+			}
+		}
+	}
+
+//	b3ContactSolverInfo info = infoGlobal;
+
+
+	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
+	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
+
+	///@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);
+	else
+		m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
+
+	m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
+	{
+		int i;
+		for (i=0;i<numNonContactPool;i++)
+		{
+			m_orderNonContactConstraintPool[i] = i;
+		}
+		for (i=0;i<numConstraintPool;i++)
+		{
+			m_orderTmpConstraintPool[i] = 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)
+{
+
+	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)
+		{
+
+			for (int j=0; j<numNonContactPool; ++j) {
+				int tmp = m_orderNonContactConstraintPool[j];
+				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)
+			{
+				for (int j=0; j<numConstraintPool; ++j) {
+					int tmp = m_orderTmpConstraintPool[j];
+					int swapi = b3RandInt2(j+1);
+					m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
+					m_orderTmpConstraintPool[swapi] = tmp;
+				}
+
+				for (int j=0; j<numFrictionPool; ++j) {
+					int tmp = m_orderFrictionConstraintPool[j];
+					int swapi = b3RandInt2(j+1);
+					m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
+					m_orderFrictionConstraintPool[swapi] = tmp;
+				}
+			}
+		}
+	}
+
+	if (infoGlobal.m_solverMode & B3_SOLVER_SIMD)
+	{
+		///solve all joint constraints, using SIMD, if available
+		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);
+		}
+
+		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;
+
+				for (int c=0;c<numPoolConstraints;c++)
+				{
+					b3Scalar totalImpulse =0;
+
+					{
+						const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
+						resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						totalImpulse = solveManifold.m_appliedImpulse;
+					}
+					bool applyFriction = true;
+					if (applyFriction)
+					{
+						{
+
+							b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
+
+							if (totalImpulse>b3Scalar(0))
+							{
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+								resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+							}
+						}
+
+						if (infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)
+						{
+
+							b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
+				
+							if (totalImpulse>b3Scalar(0))
+							{
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+								resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+							}
+						}
+					}
+				}
+
+			}
+			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++)
+				{
+					const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+					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++)
+				{
+					b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+					if (totalImpulse>b3Scalar(0))
+					{
+						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					}
+				}
+
+				
+				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+				for (j=0;j<numRollingFrictionPoolConstraints;j++)
+				{
+
+					b3SolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+					b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+					if (totalImpulse>b3Scalar(0))
+					{
+						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);
+					}
+				}
+				
+
+			}			
+		}
+	} else
+	{
+		//non-SIMD version
+		///solve all joint constraints
+		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);
+		}
+
+		if (iteration< infoGlobal.m_numIterations)
+		{
+
+			///solve all contact constraints
+			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+			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);
+			}
+			///solve all friction constraints
+			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+			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))
+				{
+					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);
+				}
+			}
+
+			int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+			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))
+				{
+					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);
+				}
+			}
+		}
+	}
+	return 0.f;
+}
+
+
+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++)
+			{
+				{
+					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+					int 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);
+					}
+				}
+			}
+		}
+		else
+		{
+			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+			{
+				{
+					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+					int 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);
+					}
+				}
+			}
+		}
+	}
+}
+
+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);
+
+		int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
+
+		for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
+		//for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
+		{			
+			
+			solveSingleIteration(iteration, constraints,numConstraints,infoGlobal);
+
+
+			if (!m_usePgs)
+			{
+				averageVelocities();
+			}
+		}
+		
+	}
+	return 0.f;
+}
+
+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_deltaAngularVelocities.resize(0);
+	m_deltaAngularVelocities.resize(numBodies,b3MakeVector3(0,0,0));
+
+	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();
+		}
+	}
+				
+	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 b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal)
+{
+	B3_PROFILE("solveGroupCacheFriendlyFinish");
+	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+	int i,j;
+
+	if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING)
+	{
+		for (j=0;j<numPoolConstraints;j++)
+		{
+			const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
+			b3ContactPoint* pt = (b3ContactPoint*) solveManifold.m_originalContactPoint;
+			b3Assert(pt);
+			pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
+		//	float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+			//	printf("pt->m_appliedImpulseLateral1 = %f\n", f);
+			pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+			//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
+			if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS))
+			{
+				pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
+			}
+			//do a callback here?
+		}
+	}
+
+	numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
+	for (j=0;j<numPoolConstraints;j++)
+	{
+		const b3SolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
+		b3TypedConstraint* constr = (b3TypedConstraint*)solverConstr.m_originalContactPoint;
+		b3JointFeedback* fb = constr->getJointFeedback();
+		if (fb)
+		{
+			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;
+			
+		}
+
+		constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
+		if (b3Fabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
+		{
+			constr->setEnabled(false);
+		}
+	}
+
+	{
+		B3_PROFILE("write back velocities and transforms");
+		for ( i=0;i<m_tmpSolverBodyPool.size();i++)
+		{
+			int bodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex;
+			//b3Assert(i==bodyIndex);
+
+			b3RigidBodyData* body = &bodies[bodyIndex];
+			if (body->m_invMass)
+			{
+				if (infoGlobal.m_splitImpulse)
+					m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
+				else
+					m_tmpSolverBodyPool[i].writebackVelocity();
+
+				if (m_usePgs)
+				{
+					body->m_linVel = m_tmpSolverBodyPool[i].m_linearVelocity;
+					body->m_angVel = m_tmpSolverBodyPool[i].m_angularVelocity;
+				} else
+				{
+					b3Scalar factor = 1.f/b3Scalar(m_bodyCount[bodyIndex]);
+
+					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());
+
+					body->m_linVel += deltaLinVel;
+					body->m_angVel += deltaAngVel;
+				}
+			
+				if (infoGlobal.m_splitImpulse)
+				{
+					body->m_pos = m_tmpSolverBodyPool[i].m_worldTransform.getOrigin();
+					b3Quaternion orn;
+					orn = m_tmpSolverBodyPool[i].m_worldTransform.getRotation();
+					body->m_quat = orn;
+				}
+			}
+		}
+	}
+
+
+	m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
+
+	m_tmpSolverBodyPool.resizeNoInitialize(0);
+	return 0.f;
+}
+
+
+
+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
new file mode 100644
index 0000000000..d2ca307fab
--- /dev/null
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h
@@ -0,0 +1,149 @@
+#ifndef B3_PGS_JACOBI_SOLVER
+#define B3_PGS_JACOBI_SOLVER
+
+
+struct b3Contact4;
+struct b3ContactPoint;
+
+
+class b3Dispatcher;
+
+#include "b3TypedConstraint.h"
+#include "b3ContactSolverInfo.h"
+#include "b3SolverBody.h"
+#include "b3SolverConstraint.h"
+
+struct b3RigidBodyData;
+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<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();
+
+	int							m_maxOverrideNumSolverIterations;
+
+	int							m_numSplitImpulseRecoveries;
+
+	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 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, 
+								b3Vector3& rel_pos1, b3Vector3& rel_pos2);
+
+	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;
+
+	
+	b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution);
+
+	void	convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal);
+
+
+	void	resolveSplitPenetrationSIMD(
+     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);
+
+	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:
+
+	virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,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);
+
+	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();
+	
+	unsigned long b3Rand2();
+
+	int b3RandInt2 (int n);
+
+	void	setRandSeed(unsigned long seed)
+	{
+		m_btSeed2 = seed;
+	}
+	unsigned long	getRandSeed() const
+	{
+		return m_btSeed2;
+	}
+
+
+
+
+};
+
+#endif //B3_PGS_JACOBI_SOLVER
+
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp
new file mode 100644
index 0000000000..02c11db320
--- /dev/null
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp
@@ -0,0 +1,209 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "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,const b3Vector3& pivotInA)
+:b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
+m_flags(0),
+m_useSolveConstraintObsolete(false)
+{
+	
+}
+*/
+
+
+void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
+{
+	getInfo1NonVirtual(info,bodies);
+}
+
+void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)
+{
+		info->m_numConstraintRows = 3;
+		info->nub = 3;
+}
+
+
+
+
+void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
+{
+	b3Transform trA;
+	trA.setIdentity();
+	trA.setOrigin(bodies[m_rbA].m_pos);
+	trA.setRotation(bodies[m_rbA].m_quat);
+
+	b3Transform trB;
+	trB.setIdentity();
+	trB.setOrigin(bodies[m_rbB].m_pos);
+	trB.setRotation(bodies[m_rbB].m_quat);
+
+	getInfo2NonVirtual(info, trA,trB);
+}
+
+void b3Point2PointConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
+{
+
+	 //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();
+	//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 a1neg = -a1;
+		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;
+	}
+	
+	b3Vector3 a2 = body1_trans.getBasis()*getPivotInB();
+   
+	{
+	//	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);
+	}
+    
+
+
+    // 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]);
+		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
+    }
+	if(m_flags & B3_P2P_FLAGS_CFM)
+	{
+		for (j=0; j<3; j++)
+		{
+			info->cfm[j*info->rowskip] = m_cfm;
+		}
+	}
+
+	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_damping = m_setting.m_damping;
+	
+}
+
+
+
+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). 
+///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)
+	{
+		b3AssertConstrParams(0);
+	}
+	else
+	{
+		switch(num)
+		{
+			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; 
+				m_flags |= B3_P2P_FLAGS_CFM;
+				break;
+			default: 
+				b3AssertConstrParams(0);
+		}
+	}
+}
+
+///return the local value of parameter
+b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const 
+{
+	b3Scalar retVal(B3_INFINITY);
+	if(axis != -1)
+	{
+		b3AssertConstrParams(0);
+	}
+	else
+	{
+		switch(num)
+		{
+			case B3_CONSTRAINT_ERP :
+			case B3_CONSTRAINT_STOP_ERP :
+				b3AssertConstrParams(m_flags & B3_P2P_FLAGS_ERP);
+				retVal = m_erp; 
+				break;
+			case B3_CONSTRAINT_CFM :
+			case B3_CONSTRAINT_STOP_CFM :
+				b3AssertConstrParams(m_flags & B3_P2P_FLAGS_CFM);
+				retVal = m_cfm; 
+				break;
+			default: 
+				b3AssertConstrParams(0);
+		}
+	}
+	return retVal;
+}
+	
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h
new file mode 100644
index 0000000000..681b487334
--- /dev/null
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h
@@ -0,0 +1,159 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_POINT2POINTCONSTRAINT_H
+#define B3_POINT2POINTCONSTRAINT_H
+
+#include "Bullet3Common/b3Vector3.h"
+//#include "b3JacobianEntry.h"
+#include "b3TypedConstraint.h"
+
+class b3RigidBody;
+
+
+#ifdef B3_USE_DOUBLE_PRECISION
+#define b3Point2PointConstraintData	b3Point2PointConstraintDoubleData
+#define b3Point2PointConstraintDataName	"b3Point2PointConstraintDoubleData"
+#else
+#define b3Point2PointConstraintData	b3Point2PointConstraintFloatData
+#define b3Point2PointConstraintDataName	"b3Point2PointConstraintFloatData"
+#endif //B3_USE_DOUBLE_PRECISION
+
+struct	b3ConstraintSetting
+{
+	b3ConstraintSetting()	:
+		m_tau(b3Scalar(0.3)),
+		m_damping(b3Scalar(1.)),
+		m_impulseClamp(b3Scalar(0.))
+	{
+	}
+	b3Scalar		m_tau;
+	b3Scalar		m_damping;
+	b3Scalar		m_impulseClamp;
+};
+
+enum b3Point2PointFlags
+{
+	B3_P2P_FLAGS_ERP = 1,
+	B3_P2P_FLAGS_CFM = 2
+};
+
+/// 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
+{
+#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();
+
+	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);
+
+	void getInfo1NonVirtual (b3ConstraintInfo1* 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	updateRHS(b3Scalar	timeStep);
+
+	void	setPivotA(const b3Vector3& pivotA)
+	{
+		m_pivotInA = pivotA;
+	}
+
+	void	setPivotB(const b3Vector3& pivotB)
+	{
+		m_pivotInB = pivotB;
+	}
+
+	const b3Vector3& getPivotInA() const
+	{
+		return m_pivotInA;
+	}
+
+	const b3Vector3& getPivotInB() const
+	{
+		return m_pivotInB;
+	}
+
+	///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);
+	///return the local value of parameter
+	virtual	b3Scalar getParam(int num, int axis = -1) 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;
+
+
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	b3Point2PointConstraintFloatData
+{
+	b3TypedConstraintData	m_typeConstraintData;
+	b3Vector3FloatData	m_pivotInA;
+	b3Vector3FloatData	m_pivotInB;
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	b3Point2PointConstraintDoubleData
+{
+	b3TypedConstraintData	m_typeConstraintData;
+	b3Vector3DoubleData	m_pivotInA;
+	b3Vector3DoubleData	m_pivotInB;
+};
+
+/*
+B3_FORCE_INLINE	int	b3Point2PointConstraint::calculateSerializeBufferSize() const
+{
+	return sizeof(b3Point2PointConstraintData);
+
+}
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+B3_FORCE_INLINE	const char*	b3Point2PointConstraint::serialize(void* dataBuffer, b3Serializer* serializer) const
+{
+	b3Point2PointConstraintData* p2pData = (b3Point2PointConstraintData*)dataBuffer;
+
+	b3TypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
+	m_pivotInA.serialize(p2pData->m_pivotInA);
+	m_pivotInB.serialize(p2pData->m_pivotInB);
+
+	return b3Point2PointConstraintDataName;
+}
+*/
+
+#endif //B3_POINT2POINTCONSTRAINT_H
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h
new file mode 100644
index 0000000000..0049317d98
--- /dev/null
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h
@@ -0,0 +1,302 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_SOLVER_BODY_H
+#define B3_SOLVER_BODY_H
+
+
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3Matrix3x3.h"
+
+#include "Bullet3Common/b3AlignedAllocator.h"
+#include "Bullet3Common/b3TransformUtil.h"
+
+///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 //
+
+
+#ifdef USE_SIMD
+
+struct	b3SimdScalar
+{
+	B3_FORCE_INLINE	b3SimdScalar()
+	{
+
+	}
+
+	B3_FORCE_INLINE	b3SimdScalar(float	fl)
+	:m_vec128 (_mm_set1_ps(fl))
+	{
+	}
+
+	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;
+	};
+	B3_FORCE_INLINE	__m128	get128()
+	{
+		return m_vec128;
+	}
+
+	B3_FORCE_INLINE	const __m128	get128() const
+	{
+		return m_vec128;
+	}
+
+	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 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) 
+{
+	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) 
+{
+	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_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;
+	};
+
+	int padding[3];
+
+
+	void	setWorldTransform(const b3Transform& worldTransform)
+	{
+		m_worldTransform = worldTransform;
+	}
+
+	const b3Transform& getWorldTransform() const
+	{
+		return m_worldTransform;
+	}
+	
+	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);
+		else
+			velocity.setValue(0,0,0);
+	}
+
+	B3_FORCE_INLINE void	getAngularVelocity(b3Vector3& angVel) const
+	{
+		if (m_originalBody)
+			angVel =m_angularVelocity+m_deltaAngularVelocity;
+		else
+			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)
+	{
+		if (m_originalBody)
+		{
+			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)
+	{
+		if (m_originalBody)
+		{
+			m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor;
+			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		}
+	}
+
+
+
+	const b3Vector3& getDeltaLinearVelocity() const
+	{
+		return m_deltaLinearVelocity;
+	}
+
+	const b3Vector3& getDeltaAngularVelocity() const
+	{
+		return m_deltaAngularVelocity;
+	}
+
+	const b3Vector3& getPushVelocity() const 
+	{
+		return m_pushVelocity;
+	}
+
+	const b3Vector3& getTurnVelocity() const 
+	{
+		return m_turnVelocity;
+	}
+
+
+	////////////////////////////////////////////////
+	///some internal methods, don't use them
+		
+	b3Vector3& internalGetDeltaLinearVelocity()
+	{
+		return m_deltaLinearVelocity;
+	}
+
+	b3Vector3& internalGetDeltaAngularVelocity()
+	{
+		return m_deltaAngularVelocity;
+	}
+
+	const b3Vector3& internalGetAngularFactor() const
+	{
+		return m_angularFactor;
+	}
+
+	const b3Vector3& internalGetInvMass() const
+	{
+		return m_invMass;
+	}
+
+	void internalSetInvMass(const b3Vector3& invMass)
+	{
+		m_invMass = invMass;
+	}
+	
+	b3Vector3& internalGetPushVelocity()
+	{
+		return m_pushVelocity;
+	}
+
+	b3Vector3& internalGetTurnVelocity()
+	{
+		return m_turnVelocity;
+	}
+
+	B3_FORCE_INLINE void	internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const
+	{
+		velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
+	}
+
+	B3_FORCE_INLINE void	internalGetAngularVelocity(b3Vector3& angVel) const
+	{
+		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)
+	{
+		//if (m_originalBody)
+		{
+			m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
+			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		}
+	}
+		
+	
+	
+
+	void	writebackVelocity()
+	{
+		//if (m_originalBody>=0)
+		{
+			m_linearVelocity +=m_deltaLinearVelocity;
+			m_angularVelocity += m_deltaAngularVelocity;
+			
+			//m_originalBody->setCompanionId(-1);
+		}
+	}
+
+
+	void	writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp)
+	{
+        (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)
+			{
+			//	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
+
+
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h
new file mode 100644
index 0000000000..bce83d4608
--- /dev/null
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h
@@ -0,0 +1,80 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_SOLVER_CONSTRAINT_H
+#define B3_SOLVER_CONSTRAINT_H
+
+
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3Matrix3x3.h"
+//#include "b3JacobianEntry.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+//#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_DECLARE_ALIGNED_ALLOCATOR();
+
+	b3Vector3		m_relpos1CrossNormal;
+	b3Vector3		m_contactNormal;
+
+	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;
+	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;
+	};
+
+	int	m_overrideNumSolverIterations;
+    int			m_frictionIndex;
+	int m_solverBodyIdA;
+	int m_solverBodyIdB;
+
+    
+	enum		b3SolverConstraintType
+	{
+		B3_SOLVER_CONTACT_1D = 0,
+		B3_SOLVER_FRICTION_1D
+	};
+};
+
+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
new file mode 100644
index 0000000000..699c481d64
--- /dev/null
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp
@@ -0,0 +1,161 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "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)
+{
+}
+
+
+
+
+b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact)
+{
+	if(lowLim > uppLim)
+	{
+		return b3Scalar(1.0f);
+	}
+	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((pos >= lowLim) && (pos < (lowLim - delta_max)))
+		{
+			lim_fact = (lowLim - pos) / delta_max;
+		}
+		else if(pos  < lowLim)
+		{
+			lim_fact = b3Scalar(0.0f);
+		}
+		else
+		{
+			lim_fact = b3Scalar(1.0f);
+		}
+	}
+	else if(delta_max > b3Scalar(0.0f))
+	{
+		if((pos <= uppLim) && (pos > (uppLim - delta_max)))
+		{
+			lim_fact = (uppLim - pos) / delta_max;
+		}
+		else if(pos  > uppLim)
+		{
+			lim_fact = b3Scalar(0.0f);
+		}
+		else
+		{
+			lim_fact = b3Scalar(1.0f);
+		}
+	}
+	else
+	{
+			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_biasFactor = _biasFactor;
+	m_relaxationFactor = _relaxationFactor;
+}
+
+void b3AngularLimit::test(const b3Scalar angle)
+{
+	m_correction = 0.0f;
+	m_sign = 0.0f;
+	m_solveLimit = false;
+
+	if (m_halfRange >= 0.0f)
+	{
+		b3Scalar deviation = b3NormalizeAngle(angle - m_center);
+		if (deviation < -m_halfRange)
+		{
+			m_solveLimit = true;
+			m_correction = - (deviation + m_halfRange);
+			m_sign = +1.0f;
+		}
+		else if (deviation > m_halfRange)
+		{
+			m_solveLimit = true;
+			m_correction = m_halfRange - deviation;
+			m_sign = -1.0f;
+		}
+	}
+}
+
+
+b3Scalar b3AngularLimit::getError() const
+{
+	return m_correction * m_sign;
+}
+
+void b3AngularLimit::fit(b3Scalar& angle) const
+{
+	if (m_halfRange > 0.0f)
+	{
+		b3Scalar relativeAngle = b3NormalizeAngle(angle - m_center);
+		if (!b3Equal(relativeAngle, m_halfRange))
+		{
+			if (relativeAngle > 0.0f)
+			{
+				angle = getHigh();
+			}
+			else
+			{
+				angle = getLow();
+			}
+		}
+	}
+}
+
+b3Scalar b3AngularLimit::getLow() const
+{
+	return b3NormalizeAngle(m_center - m_halfRange);
+}
+
+b3Scalar b3AngularLimit::getHigh() const
+{
+	return b3NormalizeAngle(m_center + m_halfRange);
+}
diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h
new file mode 100644
index 0000000000..cf9cec0d5e
--- /dev/null
+++ b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h
@@ -0,0 +1,483 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2010 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_TYPED_CONSTRAINT_H
+#define B3_TYPED_CONSTRAINT_H
+
+
+#include "Bullet3Common/b3Scalar.h"
+#include "b3SolverConstraint.h"
+
+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_HINGE_CONSTRAINT_TYPE,
+	B3_CONETWIST_CONSTRAINT_TYPE,
+	B3_D6_CONSTRAINT_TYPE,
+	B3_SLIDER_CONSTRAINT_TYPE,
+	B3_CONTACT_CONSTRAINT_TYPE,
+	B3_D6_SPRING_CONSTRAINT_TYPE,
+	B3_GEAR_CONSTRAINT_TYPE,
+	B3_FIXED_CONSTRAINT_TYPE,
+	B3_MAX_CONSTRAINT_TYPE
+};
+
+
+enum b3ConstraintParams
+{
+	B3_CONSTRAINT_ERP=1,
+	B3_CONSTRAINT_STOP_ERP,
+	B3_CONSTRAINT_CFM,
+	B3_CONSTRAINT_STOP_CFM
+};
+
+#if 1
+	#define b3AssertConstrParams(_par) b3Assert(_par) 
+#else
+	#define b3AssertConstrParams(_par)
+#endif
+
+
+B3_ATTRIBUTE_ALIGNED16(struct)	b3JointFeedback
+{
+	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
+{
+	int	m_userConstraintType;
+
+	union
+	{
+		int	m_userConstraintId;
+		void* m_userConstraintPtr;
+	};
+
+	b3Scalar	m_breakingImpulseThreshold;
+	bool		m_isEnabled;
+	bool		m_needsFeedback;
+	int			m_overrideNumSolverIterations;
+
+
+	b3TypedConstraint&	operator=(b3TypedConstraint&	other)
+	{
+		b3Assert(0);
+		(void) other;
+		return *this;
+	}
+
+protected:
+	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);
+
+	struct b3ConstraintInfo1 {
+		int m_numConstraintRows,nub;
+	};
+
+	
+
+	struct b3ConstraintInfo2 {
+		// integrator parameters: frames per second (1/stepsize), default error
+		// reduction parameter (0..1).
+		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;
+
+		// elements to jump from one row to the next in J's
+		int rowskip;
+
+		// 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;
+
+		// lo and hi limits for variables (set to -/+ infinity on entry).
+		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;
+		// number of solver iterations
+		int m_numIterations;
+
+		//damping of the velocity
+		b3Scalar	m_damping;
+	};
+
+	int	getOverrideNumSolverIterations() const
+	{
+		return m_overrideNumSolverIterations;
+	}
+
+	///override the number of constraint solver iterations used to solve this constraint
+	///-1 will use the default number of iterations, as specified in SolverInfo.m_numIterations
+	void setOverrideNumSolverIterations(int overideNumIterations)
+	{
+		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)
+	{
+        (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;
+
+	///internal method used by the constraint solver, don't use them directly
+	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)
+	{
+		m_appliedImpulse = appliedImpulse;
+	}
+	///internal method used by the constraint solver, don't use them directly
+	b3Scalar	internalGetAppliedImpulse()
+	{
+		return m_appliedImpulse;
+	}
+
+
+	b3Scalar	getBreakingImpulseThreshold() const
+	{
+		return 	m_breakingImpulseThreshold;
+	}
+
+	void	setBreakingImpulseThreshold(b3Scalar threshold)
+	{
+		m_breakingImpulseThreshold = threshold;
+	}
+
+	bool	isEnabled() const
+	{
+		return m_isEnabled;
+	}
+
+	void	setEnabled(bool 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*/) {};
+
+	
+	int getRigidBodyA() const
+	{
+		return m_rbA;
+	}
+	int getRigidBodyB() const
+	{
+		return m_rbB;
+	}
+
+
+	int getRigidBodyA() 
+	{
+		return m_rbA;
+	}
+	int getRigidBodyB()
+	{
+		return m_rbB;
+	}
+
+	int getUserConstraintType() const
+	{
+		return m_userConstraintType ;
+	}
+
+	void	setUserConstraintType(int userConstraintType)
+	{
+		m_userConstraintType = userConstraintType;
+	};
+
+	void	setUserConstraintId(int uid)
+	{
+		m_userConstraintId = uid;
+	}
+
+	int getUserConstraintId() const
+	{
+		return m_userConstraintId;
+	}
+
+	void	setUserConstraintPtr(void* ptr)
+	{
+		m_userConstraintPtr = ptr;
+	}
+
+	void*	getUserConstraintPtr()
+	{
+		return m_userConstraintPtr;
+	}
+
+	void	setJointFeedback(b3JointFeedback* jointFeedback)
+	{
+		m_jointFeedback = jointFeedback;
+	}
+
+	const b3JointFeedback* getJointFeedback() const
+	{
+		return m_jointFeedback;
+	}
+
+	b3JointFeedback* getJointFeedback()
+	{
+		return m_jointFeedback;
+	}
+
+
+	int getUid() const
+	{
+		return m_userConstraintId;   
+	} 
+
+	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)
+	{
+		m_needsFeedback = needsFeedback;
+	}
+
+	///getAppliedImpulse is an estimated total applied impulse. 
+	///This feedback could be used to determine breaking constraints or playing sounds.
+	b3Scalar	getAppliedImpulse() const
+	{
+		b3Assert(m_needsFeedback);
+		return m_appliedImpulse;
+	}
+
+	b3TypedConstraintType getConstraintType () const
+	{
+		return b3TypedConstraintType(m_objectType);
+	}
+	
+	void setDbgDrawSize(b3Scalar dbgDrawSize)
+	{
+		m_dbgDrawSize = dbgDrawSize;
+	}
+	b3Scalar getDbgDrawSize()
+	{
+		return m_dbgDrawSize;
+	}
+
+	///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;
+
+	///return the local value of parameter
+	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 
+// 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)
+	{
+		return angleInRadians;
+	}
+	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)
+	{
+		b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleInRadians - angleUpperLimitInRadians));
+		b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleInRadians - angleLowerLimitInRadians));
+		return (diffLo < diffHi) ? (angleInRadians - B3_2_PI) : angleInRadians;
+	}
+	else
+	{
+		return angleInRadians;
+	}
+}
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	b3TypedConstraintData
+{
+	int		m_bodyA;
+	int		m_bodyB;
+	char	*m_name;
+
+	int	m_objectType;
+	int	m_userConstraintType;
+	int	m_userConstraintId;
+	int	m_needsFeedback;
+
+	float	m_appliedImpulse;
+	float	m_dbgDrawSize;
+
+	int	m_disableCollisionsBetweenLinkedBodies;
+	int	m_overrideNumSolverIterations;
+
+	float	m_breakingImpulseThreshold;
+	int		m_isEnabled;
+	
+};
+
+/*B3_FORCE_INLINE	int	b3TypedConstraint::calculateSerializeBufferSize() const
+{
+	return sizeof(b3TypedConstraintData);
+}
+*/
+
+
+class b3AngularLimit
+{
+private:
+	b3Scalar 
+		m_center,
+		m_halfRange,
+		m_softness,
+		m_biasFactor,
+		m_relaxationFactor,
+		m_correction,
+		m_sign;
+
+	bool
+		m_solveLimit;
+
+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)
+	{}
+
+	/// Sets all limit's parameters.
+	/// When low > high limit becomes inactive.
+	/// When high - low > 2PI limit is ineffective too becouse no angle can exceed the limit
+	void set(b3Scalar low, b3Scalar high, b3Scalar _softness = 0.9f, b3Scalar _biasFactor = 0.3f, b3Scalar _relaxationFactor = 1.0f);
+
+	/// Checks conastaint angle against limit. If limit is active and the angle violates the limit
+	/// correction is calculated.
+	void test(const b3Scalar angle);
+
+	/// Returns limit's softness
+	inline b3Scalar getSoftness() const
+	{
+		return m_softness;
+	}
+
+	/// Returns limit's bias factor
+	inline b3Scalar getBiasFactor() const
+	{
+		return m_biasFactor;
+	}
+
+	/// Returns limit's relaxation factor
+	inline b3Scalar getRelaxationFactor() const
+	{
+		return m_relaxationFactor;
+	}
+
+	/// Returns correction value evaluated when test() was invoked 
+	inline b3Scalar getCorrection() const
+	{
+		return m_correction;
+	}
+
+	/// Returns sign value evaluated when test() was invoked 
+	inline b3Scalar getSign() const
+	{
+		return m_sign;
+	}
+
+	/// Gives half of the distance between min and max limit angle
+	inline b3Scalar getHalfRange() const
+	{
+		return m_halfRange;
+	}
+
+	/// Returns true when the last test() invocation recognized limit violation
+	inline bool isLimit() const
+	{
+		return m_solveLimit;
+	}
+
+	/// Checks given angle against limit. If limit is active and angle doesn't fit it, the angle
+	/// returned is modified so it equals to the limit closest to given angle.
+	void fit(b3Scalar& angle) const;
+
+	/// Returns correction value multiplied by sign value
+	b3Scalar getError() const;
+
+	b3Scalar getLow() const;
+
+	b3Scalar getHigh() const;
+
+};
+
+
+
+#endif //B3_TYPED_CONSTRAINT_H