Remove unused Bullet module and thirdparty code

It has been disabled in `master` since one year (#45852) and our plan
is for Bullet, and possibly other thirdparty physics engines, to be
implemented via GDExtension so that they can be selected by the users
who need them.

14 files changed, 0 insertions, 4836 deletions

diff --git a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h b/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h
deleted file mode 100644
index 049c9116fd..0000000000
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h
+++ /dev/null
@@ -1,149 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#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
deleted file mode 100644
index ace4b18388..0000000000
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp
+++ /dev/null
@@ -1,103 +0,0 @@
-
-#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
deleted file mode 100644
index 64809666e4..0000000000
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h
+++ /dev/null
@@ -1,34 +0,0 @@
-
-#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
deleted file mode 100644
index 0d5bb2014b..0000000000
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp
+++ /dev/null
@@ -1,737 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-/*
-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->fps;
-				info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
-			}
-		}
-		if (limit)
-		{
-			b3Scalar k = info->fps * limot->m_stopERP;
-			if (!rotational)
-			{
-				info->m_constraintError[srow] += k * limot->m_currentLimitError;
-			}
-			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
deleted file mode 100644
index 1597809db3..0000000000
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h
+++ /dev/null
@@ -1,517 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-/// 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 = 6.0f;
-		m_maxLimitForce = 300.0f;
-		m_loLimit = 1.0f;
-		m_hiLimit = -1.0f;
-		m_normalCFM = 0.f;
-		m_stopERP = 0.2f;
-		m_stopCFM = 0.f;
-		m_bounce = 0.0f;
-		m_damping = 1.0f;
-		m_limitSoftness = 0.5f;
-		m_currentLimit = 0;
-		m_currentLimitError = 0;
-		m_enableMotor = false;
-	}
-
-	b3RotationalLimitMotor(const b3RotationalLimitMotor& limot)
-	{
-		m_targetVelocity = limot.m_targetVelocity;
-		m_maxMotorForce = limot.m_maxMotorForce;
-		m_limitSoftness = limot.m_limitSoftness;
-		m_loLimit = limot.m_loLimit;
-		m_hiLimit = limot.m_hiLimit;
-		m_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
deleted file mode 100644
index 13269debf6..0000000000
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h
+++ /dev/null
@@ -1,150 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#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
deleted file mode 100644
index b7050b1070..0000000000
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp
+++ /dev/null
@@ -1,1696 +0,0 @@
-/*
-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
deleted file mode 100644
index 5b616541d9..0000000000
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h
+++ /dev/null
@@ -1,133 +0,0 @@
-#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
deleted file mode 100644
index cfa7c7dd11..0000000000
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp
+++ /dev/null
@@ -1,190 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#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
deleted file mode 100644
index 14762a3e35..0000000000
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h
+++ /dev/null
@@ -1,153 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#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
deleted file mode 100644
index 196d0e5793..0000000000
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h
+++ /dev/null
@@ -1,281 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#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
deleted file mode 100644
index 4927ae4288..0000000000
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#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
deleted file mode 100644
index 885e277d8c..0000000000
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp
+++ /dev/null
@@ -1,151 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose, 
-including commercial applications, and to alter it and redistribute it freely, 
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#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
deleted file mode 100644
index f74aec4d3c..0000000000
--- a/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h
+++ /dev/null
@@ -1,469 +0,0 @@
-/*
-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;
-	}
-}
-
-// clang-format off
-///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;
-	
-};
-
-// clang-format on
-
-/*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