text: /*
  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.
  */
  
  //#define COMPUTE_IMPULSE_DENOM 1
  #ifdef BT_DEBUG
  #	define BT_ADDITIONAL_DEBUG
  #endif
  
  //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
  
  #include "btSequentialImpulseConstraintSolver.h"
  #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
  
  #include "LinearMath/btIDebugDraw.h"
  #include "LinearMath/btCpuFeatureUtility.h"
  
  //#include "btJacobianEntry.h"
  #include "LinearMath/btMinMax.h"
  #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
  #include <new>
  #include "LinearMath/btStackAlloc.h"
  #include "LinearMath/btQuickprof.h"
  //#include "btSolverBody.h"
  //#include "btSolverConstraint.h"
  #include "LinearMath/btAlignedObjectArray.h"
  #include <string.h>  //for memset
  
  int gNumSplitImpulseRecoveries = 0;
  
  #include "BulletDynamics/Dynamics/btRigidBody.h"
  
  //#define VERBOSE_RESIDUAL_PRINTF 1
  ///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver
  ///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check.
  static btScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
  {
  	btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm;
  	const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity());
  	const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity());
  
  	//	const btScalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;
  	deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
  	deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
  
  	const btScalar sum = btScalar(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;
  	}
  
  	bodyA.internalApplyImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
  	bodyB.internalApplyImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
  
  	return deltaImpulse * (1. / c.m_jacDiagABInv);
  }
  
  static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
  {
  	btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm;
  	const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity());
  	const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity());
  
  	deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
  	deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
  	const btScalar sum = btScalar(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;
  	}
  	bodyA.internalApplyImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
  	bodyB.internalApplyImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
  
  	return deltaImpulse * (1. / c.m_jacDiagABInv);
  }
  
  #ifdef USE_SIMD
  #include <emmintrin.h>
  
  #define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e, e, e, e))
  static inline __m128 btSimdDot3(__m128 vec0, __m128 vec1)
  {
  	__m128 result = _mm_mul_ps(vec0, vec1);
  	return _mm_add_ps(btVecSplat(result, 0), _mm_add_ps(btVecSplat(result, 1), btVecSplat(result, 2)));
  }
  
  #if defined(BT_ALLOW_SSE4)
  #include <intrin.h>
  
  #define USE_FMA 1
  #define USE_FMA3_INSTEAD_FMA4 1
  #define USE_SSE4_DOT 1
  
  #define SSE4_DP(a, b) _mm_dp_ps(a, b, 0x7f)
  #define SSE4_DP_FP(a, b) _mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f))
  
  #if USE_SSE4_DOT
  #define DOT_PRODUCT(a, b) SSE4_DP(a, b)
  #else
  #define DOT_PRODUCT(a, b) btSimdDot3(a, b)
  #endif
  
  #if USE_FMA
  #if USE_FMA3_INSTEAD_FMA4
  // a*b + c
  #define FMADD(a, b, c) _mm_fmadd_ps(a, b, c)
  // -(a*b) + c
  #define FMNADD(a, b, c) _mm_fnmadd_ps(a, b, c)
  #else  // USE_FMA3
  // a*b + c
  #define FMADD(a, b, c) _mm_macc_ps(a, b, c)
  // -(a*b) + c
  #define FMNADD(a, b, c) _mm_nmacc_ps(a, b, c)
  #endif
  #else  // USE_FMA
  // c + a*b
  #define FMADD(a, b, c) _mm_add_ps(c, _mm_mul_ps(a, b))
  // c - a*b
  #define FMNADD(a, b, c) _mm_sub_ps(c, _mm_mul_ps(a, b))
  #endif
  #endif
  
  // Project Gauss Seidel or the equivalent Sequential Impulse
  static btScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
  {
  	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
  	__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
  	__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
  	btSimdScalar 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(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
  	__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().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)));
  	btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
  	btSimdScalar 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_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
  	__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, bodyB.internalGetInvMass().mVec128);
  	__m128 impulseMagnitude = deltaImpulse;
  	bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
  	bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
  	bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
  	bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
  	return deltaImpulse.m_floats[0] / c.m_jacDiagABInv;
  }
  
  // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
  static btScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
  {
  #if defined(BT_ALLOW_SSE4)
  	__m128 tmp = _mm_set_ps1(c.m_jacDiagABInv);
  	__m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm);
  	const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit);
  	const __m128 upperLimit = _mm_set_ps1(c.m_upperLimit);
  	const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
  	const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
  	deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
  	deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
  	tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse);  // sum
  	const __m128 maskLower = _mm_cmpgt_ps(tmp, lowerLimit);
  	const __m128 maskUpper = _mm_cmpgt_ps(upperLimit, tmp);
  	deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower);
  	c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower);
  	bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128);
  	bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128);
  	bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128);
  	bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128);
  	btSimdScalar deltaImp = deltaImpulse;
  	return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv);
  #else
  	return gResolveSingleConstraintRowGeneric_sse2(bodyA, bodyB, c);
  #endif
  }
  
  static btScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
  {
  	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
  	__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
  	__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
  	btSimdScalar 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(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
  	__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().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)));
  	btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
  	btSimdScalar 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_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
  	__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128);
  	__m128 impulseMagnitude = deltaImpulse;
  	bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
  	bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
  	bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
  	bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
  	return deltaImpulse.m_floats[0] / c.m_jacDiagABInv;
  }
  
  // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
  static btScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
  {
  #ifdef BT_ALLOW_SSE4
  	__m128 tmp = _mm_set_ps1(c.m_jacDiagABInv);
  	__m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm);
  	const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit);
  	const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128));
  	const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128));
  	deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
  	deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
  	tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse);
  	const __m128 mask = _mm_cmpgt_ps(tmp, lowerLimit);
  	deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask);
  	c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, tmp, mask);
  	bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128);
  	bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128);
  	bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128);
  	bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128);
  	btSimdScalar deltaImp = deltaImpulse;
  	return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv);
  #else
  	return gResolveSingleConstraintRowLowerLimit_sse2(bodyA, bodyB, c);
  #endif  //BT_ALLOW_SSE4
  }
  
  #endif  //USE_SIMD
  
  btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
  {
  	return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c);
  }
  
  // Project Gauss Seidel or the equivalent Sequential Impulse
  btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
  {
  	return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c);
  }
  
  btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
  {
  	return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c);
  }
  
  btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
  {
  	return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c);
  }
  
  static btScalar gResolveSplitPenetrationImpulse_scalar_reference(
  	btSolverBody& bodyA,
  	btSolverBody& bodyB,
  	const btSolverConstraint& c)
  {
  	btScalar deltaImpulse = 0.f;
  
  	if (c.m_rhsPenetration)
  	{
  		gNumSplitImpulseRecoveries++;
  		deltaImpulse = c.m_rhsPenetration - btScalar(c.m_appliedPushImpulse) * c.m_cfm;
  		const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetTurnVelocity());
  		const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetTurnVelocity());
  
  		deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
  		deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
  		const btScalar sum = btScalar(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;
  		}
  		bodyA.internalApplyPushImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
  		bodyB.internalApplyPushImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
  	}
  	return deltaImpulse * (1. / c.m_jacDiagABInv);
  }
  
  static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c)
  {
  #ifdef USE_SIMD
  	if (!c.m_rhsPenetration)
  		return 0.f;
  
  	gNumSplitImpulseRecoveries++;
  
  	__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(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetTurnVelocity().mVec128));
  	__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetTurnVelocity().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)));
  	btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
  	btSimdScalar 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_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128);
  	__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128);
  	__m128 impulseMagnitude = deltaImpulse;
  	bodyA.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyA.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
  	bodyA.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyA.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
  	bodyB.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyB.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
  	bodyB.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyB.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
  	btSimdScalar deltaImp = deltaImpulse;
  	return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv);
  #else
  	return gResolveSplitPenetrationImpulse_scalar_reference(bodyA, bodyB, c);
  #endif
  }
  
  btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
  {
  	m_btSeed2 = 0;
  	m_cachedSolverMode = 0;
  	setupSolverFunctions(false);
  }
  
  void btSequentialImpulseConstraintSolver::setupSolverFunctions(bool useSimd)
  {
  	m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_scalar_reference;
  	m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_scalar_reference;
  	m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_scalar_reference;
  
  	if (useSimd)
  	{
  #ifdef USE_SIMD
  		m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
  		m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse2;
  		m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_sse2;
  
  #ifdef BT_ALLOW_SSE4
  		int cpuFeatures = btCpuFeatureUtility::getCpuFeatures();
  		if ((cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_FMA3) && (cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_SSE4_1))
  		{
  			m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3;
  			m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
  		}
  #endif  //BT_ALLOW_SSE4
  #endif  //USE_SIMD
  	}
  }
  
  btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
  {
  }
  
  btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric()
  {
  	return gResolveSingleConstraintRowGeneric_scalar_reference;
  }
  
  btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit()
  {
  	return gResolveSingleConstraintRowLowerLimit_scalar_reference;
  }
  
  #ifdef USE_SIMD
  btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric()
  {
  	return gResolveSingleConstraintRowGeneric_sse2;
  }
  btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit()
  {
  	return gResolveSingleConstraintRowLowerLimit_sse2;
  }
  #ifdef BT_ALLOW_SSE4
  btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric()
  {
  	return gResolveSingleConstraintRowGeneric_sse4_1_fma3;
  }
  btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit()
  {
  	return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
  }
  #endif  //BT_ALLOW_SSE4
  #endif  //USE_SIMD
  
  unsigned long btSequentialImpulseConstraintSolver::btRand2()
  {
  	m_btSeed2 = (1664525L * m_btSeed2 + 1013904223L) & 0xffffffff;
  	return m_btSeed2;
  }
  
  //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
  int btSequentialImpulseConstraintSolver::btRandInt2(int n)
  {
  	// seems good; xor-fold and modulus
  	const unsigned long un = static_cast<unsigned long>(n);
  	unsigned long r = btRand2();
  
  	// 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 btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep)
  {
  	btRigidBody* rb = collisionObject ? btRigidBody::upcast(collisionObject) : 0;
  
  	solverBody->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f);
  	solverBody->internalGetDeltaAngularVelocity().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 = rb->getWorldTransform();
  		solverBody->internalSetInvMass(btVector3(rb->getInvMass(), rb->getInvMass(), rb->getInvMass()) * rb->getLinearFactor());
  		solverBody->m_originalBody = rb;
  		solverBody->m_angularFactor = rb->getAngularFactor();
  		solverBody->m_linearFactor = rb->getLinearFactor();
  		solverBody->m_linearVelocity = rb->getLinearVelocity();
  		solverBody->m_angularVelocity = rb->getAngularVelocity();
  		solverBody->m_externalForceImpulse = rb->getTotalForce() * rb->getInvMass() * timeStep;
  		solverBody->m_externalTorqueImpulse = rb->getTotalTorque() * rb->getInvInertiaTensorWorld() * timeStep;
  	}
  	else
  		{
  		solverBody->m_worldTransform.setIdentity();
  		solverBody->internalSetInvMass(btVector3(0, 0, 0));
  		solverBody->m_originalBody = 0;
  		solverBody->m_angularFactor.setValue(1, 1, 1);
  		solverBody->m_linearFactor.setValue(1, 1, 1);
  		solverBody->m_linearVelocity.setValue(0, 0, 0);
  		solverBody->m_angularVelocity.setValue(0, 0, 0);
  		solverBody->m_externalForceImpulse.setValue(0, 0, 0);
  		solverBody->m_externalTorqueImpulse.setValue(0, 0, 0);
  					}
  				}
  
  btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
  {
  	//printf("rel_vel =%f\n", rel_vel);
  	if (btFabs(rel_vel) < velocityThreshold)
  		return 0.;
  
  	btScalar rest = restitution * -rel_vel;
  	return rest;
  }
  
  void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj, btVector3& frictionDirection, int frictionMode)
  {
  	if (colObj && colObj->hasAnisotropicFriction(frictionMode))
  	{
  		// transform to local coordinates
  		btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
  		const btVector3& friction_scaling = colObj->getAnisotropicFriction();
  		//apply anisotropic friction
  		loc_lateral *= friction_scaling;
  		// ... and transform it back to global coordinates
  		frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
  	}
  }
  
  void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
  {
  	btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
  	btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
  
  	btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
  	btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
  
  	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;
  
  	if (body0)
  	{
  		solverConstraint.m_contactNormal1 = normalAxis;
  		btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
  		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
  		solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld() * ftorqueAxis1 * body0->getAngularFactor();
  	}
  	else
  	{
  		solverConstraint.m_contactNormal1.setZero();
  		solverConstraint.m_relpos1CrossNormal.setZero();
  		solverConstraint.m_angularComponentA.setZero();
  	}
  
  	if (bodyA)
  	{
  		solverConstraint.m_contactNormal2 = -normalAxis;
  		btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
  		solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
  		solverConstraint.m_angularComponentB = bodyA->getInvInertiaTensorWorld() * ftorqueAxis1 * bodyA->getAngularFactor();
  	}
  	else
  	{
  		solverConstraint.m_contactNormal2.setZero();
  		solverConstraint.m_relpos2CrossNormal.setZero();
  		solverConstraint.m_angularComponentB.setZero();
  	}
  
  	{
  		btVector3 vec;
  		btScalar denom0 = 0.f;
  		btScalar denom1 = 0.f;
  		if (body0)
  		{
  			vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
  			denom0 = body0->getInvMass() + normalAxis.dot(vec);
  		}
  		if (bodyA)
  		{
  			vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
  			denom1 = bodyA->getInvMass() + normalAxis.dot(vec);
  		}
  		btScalar denom = relaxation / (denom0 + denom1);
  		solverConstraint.m_jacDiagABInv = denom;
  	}
  
  	{
  		btScalar rel_vel;
  		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0 ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : btVector3(0, 0, 0));
  		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(bodyA ? solverBodyB.m_angularVelocity : btVector3(0, 0, 0));
  
  		rel_vel = vel1Dotn + vel2Dotn;
  
  		//		btScalar positionalError = 0.f;
  
  		btScalar velocityError = desiredVelocity - rel_vel;
  		btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
  
  		btScalar penetrationImpulse = btScalar(0);
  
  		if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
  		{
  			btScalar distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(normalAxis);
  			btScalar positionalError = -distance * infoGlobal.m_frictionERP / infoGlobal.m_timeStep;
  			penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
  		}
  
  		solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
  		solverConstraint.m_rhsPenetration = 0.f;
  		solverConstraint.m_cfm = cfmSlip;
  		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
  		solverConstraint.m_upperLimit = solverConstraint.m_friction;
  	}
  }
  
  btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
  {
  	btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
  	solverConstraint.m_frictionIndex = frictionIndex;
  	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
  		colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
  	return solverConstraint;
  }
  
  void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB,
  	btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
  	btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
  	btScalar desiredVelocity, btScalar cfmSlip)
  
  {
  	btVector3 normalAxis(0, 0, 0);
  
  	solverConstraint.m_contactNormal1 = normalAxis;
  	solverConstraint.m_contactNormal2 = -normalAxis;
  	btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
  	btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
  
  	btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
  	btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
  
  	solverConstraint.m_solverBodyIdA = solverBodyIdA;
  	solverConstraint.m_solverBodyIdB = solverBodyIdB;
  
  	solverConstraint.m_friction = combinedTorsionalFriction;
  	solverConstraint.m_originalContactPoint = 0;
  
  	solverConstraint.m_appliedImpulse = 0.f;
  	solverConstraint.m_appliedPushImpulse = 0.f;
  
  	{
  		btVector3 ftorqueAxis1 = -normalAxis1;
  		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
  		solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld() * ftorqueAxis1 * body0->getAngularFactor() : btVector3(0, 0, 0);
  	}
  	{
  		btVector3 ftorqueAxis1 = normalAxis1;
  		solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
  		solverConstraint.m_angularComponentB = bodyA ? bodyA->getInvInertiaTensorWorld() * ftorqueAxis1 * bodyA->getAngularFactor() : btVector3(0, 0, 0);
  	}
  
  	{
  		btVector3 iMJaA = body0 ? body0->getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3(0, 0, 0);
  		btVector3 iMJaB = bodyA ? bodyA->getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3(0, 0, 0);
  		btScalar sum = 0;
  		sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
  		sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
  		solverConstraint.m_jacDiagABInv = btScalar(1.) / sum;
  	}
  
  	{
  		btScalar rel_vel;
  		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0 ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : btVector3(0, 0, 0));
  		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(bodyA ? solverBodyB.m_angularVelocity : btVector3(0, 0, 0));
  
  		rel_vel = vel1Dotn + vel2Dotn;
  
  		//		btScalar positionalError = 0.f;
  
  		btSimdScalar velocityError = desiredVelocity - rel_vel;
  		btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
  		solverConstraint.m_rhs = velocityImpulse;
  		solverConstraint.m_cfm = cfmSlip;
  		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
  		solverConstraint.m_upperLimit = solverConstraint.m_friction;
  	}
  }
  
  btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
  {
  	btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
  	solverConstraint.m_frictionIndex = frictionIndex;
  	setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction, rel_pos1, rel_pos2,
  		colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
  	return solverConstraint;
  }
  
  int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body, btScalar timeStep)
  {
  #if BT_THREADSAFE
  	int solverBodyId = -1;
  	const bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
  	const bool isStaticOrKinematic = body.isStaticOrKinematicObject();
  	const bool isKinematic = body.isKinematicObject();
  	if (isRigidBodyType && !isStaticOrKinematic)
  	{
  		// dynamic body
  		// Dynamic bodies can only be in one island, so it's safe to write to the companionId
  		solverBodyId = body.getCompanionId();
  		if (solverBodyId < 0)
  		{
  			solverBodyId = m_tmpSolverBodyPool.size();
  			btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
  			initSolverBody(&solverBody, &body, timeStep);
  			body.setCompanionId(solverBodyId);
  		}
  	}
  	else if (isRigidBodyType && isKinematic)
  	{
  		//
  		// NOTE: must test for kinematic before static because some kinematic objects also
  		//   identify as "static"
  		//
  		// Kinematic bodies can be in multiple islands at once, so it is a
  		// race condition to write to them, so we use an alternate method
  		// to record the solverBodyId
  		int uniqueId = body.getWorldArrayIndex();
  		const int INVALID_SOLVER_BODY_ID = -1;
  		if (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size())
  		{
  			m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
  		}
  		solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId];
  		// if no table entry yet,
  		if (solverBodyId == INVALID_SOLVER_BODY_ID)
  		{
  			// create a table entry for this body
  			solverBodyId = m_tmpSolverBodyPool.size();
  			btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
  			initSolverBody(&solverBody, &body, timeStep);
  			m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId] = solverBodyId;
  		}
  	}
  	else
  	{
  		bool isMultiBodyType = (body.getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK);
  		// Incorrectly set collision object flags can degrade performance in various ways.
  		if (!isMultiBodyType)
  		{
  			btAssert(body.isStaticOrKinematicObject());
  		}
  		//it could be a multibody link collider
  		// all fixed bodies (inf mass) get mapped to a single solver id
  		if (m_fixedBodyId < 0)
  		{
  			m_fixedBodyId = m_tmpSolverBodyPool.size();
  			btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
  			initSolverBody(&fixedBody, 0, timeStep);
  		}
  		solverBodyId = m_fixedBodyId;
  	}
  	btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size());
  	return solverBodyId;
  #else   // BT_THREADSAFE
  
  	int solverBodyIdA = -1;
  
  	if (body.getCompanionId() >= 0)
  	{
  		//body has already been converted
  		solverBodyIdA = body.getCompanionId();
  		btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
  	}
  	else
  	{
  		btRigidBody* rb = btRigidBody::upcast(&body);
  		//convert both active and kinematic objects (for their velocity)
  		if (rb && (rb->getInvMass() || rb->isKinematicObject()))
  		{
  			solverBodyIdA = m_tmpSolverBodyPool.size();
  			btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
  			initSolverBody(&solverBody, &body, timeStep);
  			body.setCompanionId(solverBodyIdA);
  		}
  		else
  		{
  			if (m_fixedBodyId < 0)
  			{
  				m_fixedBodyId = m_tmpSolverBodyPool.size();
  				btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
  				initSolverBody(&fixedBody, 0, timeStep);
  			}
  			return m_fixedBodyId;
  			//			return 0;//assume first one is a fixed solver body
  		}
  	}
  
  	return solverBodyIdA;
  #endif  // BT_THREADSAFE
  }
  #include <stdio.h>
  
  void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
  	int solverBodyIdA, int solverBodyIdB,
  	btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
  	btScalar& relaxation,
  	const btVector3& rel_pos1, const btVector3& rel_pos2)
  {
  	//	const btVector3& pos1 = cp.getPositionWorldOnA();
  	//	const btVector3& pos2 = cp.getPositionWorldOnB();
  
  	btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
  	btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
  
  	btRigidBody* rb0 = bodyA->m_originalBody;
  	btRigidBody* rb1 = bodyB->m_originalBody;
  
  	//			btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
  	//			btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
  	//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
  	//rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
  
  	relaxation = infoGlobal.m_sor;
  	btScalar invTimeStep = btScalar(1) / infoGlobal.m_timeStep;
  
  	//cfm = 1 /       ( dt * kp + kd )
  	//erp = dt * kp / ( dt * kp + kd )
  
  	btScalar cfm = infoGlobal.m_globalCfm;
  	btScalar erp = infoGlobal.m_erp2;
  
  	if ((cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP))
  	{
  		if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM)
  			cfm = cp.m_contactCFM;
  		if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP)
  			erp = cp.m_contactERP;
  	}
  	else
  	{
  		if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
  		{
  			btScalar denom = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1);
  			if (denom < SIMD_EPSILON)
  			{
  				denom = SIMD_EPSILON;
  			}
  			cfm = btScalar(1) / denom;
  			erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
  		}
  	}
  
  	cfm *= invTimeStep;
  
  	btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
  	solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
  	btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
  	solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
  
  	{
  #ifdef COMPUTE_IMPULSE_DENOM
  		btScalar denom0 = rb0->computeImpulseDenominator(pos1, cp.m_normalWorldOnB);
  		btScalar denom1 = rb1->computeImpulseDenominator(pos2, cp.m_normalWorldOnB);
  #else
  		btVector3 vec;
  		btScalar denom0 = 0.f;
  		btScalar denom1 = 0.f;
  		if (rb0)
  		{
  			vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
  			denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
  		}
  		if (rb1)
  		{
  			vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
  			denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
  		}
  #endif  //COMPUTE_IMPULSE_DENOM
  
  		btScalar denom = relaxation / (denom0 + denom1 + cfm);
  		solverConstraint.m_jacDiagABInv = denom;
  	}
  
  	if (rb0)
  	{
  		solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
  		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
  	}
  	else
  	{
  		solverConstraint.m_contactNormal1.setZero();
  		solverConstraint.m_relpos1CrossNormal.setZero();
  	}
  	if (rb1)
  	{
  		solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
  		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
  	}
  	else
  	{
  		solverConstraint.m_contactNormal2.setZero();
  		solverConstraint.m_relpos2CrossNormal.setZero();
  	}
  
  	btScalar restitution = 0.f;
  	btScalar penetration = cp.getDistance() + infoGlobal.m_linearSlop;
  
  	{
  		btVector3 vel1, vel2;
  
  		vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0, 0, 0);
  		vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0, 0, 0);
  
  		//			btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
  		btVector3 vel = vel1 - vel2;
  		btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
  
  		solverConstraint.m_friction = cp.m_combinedFriction;
  
  		restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
  		if (restitution <= btScalar(0.))
  		{
  			restitution = 0.f;
  		};
  	}
  
  	///warm starting (or zero if disabled)
  	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
  	{
  		solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
  		if (rb0)
  			bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
  		if (rb1)
  			bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
  	}
  	else
  	{
  		solverConstraint.m_appliedImpulse = 0.f;
  	}
  
  	solverConstraint.m_appliedPushImpulse = 0.f;
  
  	{
  		btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse : btVector3(0, 0, 0);
  		btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse : btVector3(0, 0, 0);
  		btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse : btVector3(0, 0, 0);
  		btVector3 externalTorqueImpulseB = bodyB->m_originalBody ? bodyB->m_externalTorqueImpulse : btVector3(0, 0, 0);
  
  		btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity + externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity + externalTorqueImpulseA);
  		btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity + externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity + externalTorqueImpulseB);
  		btScalar rel_vel = vel1Dotn + vel2Dotn;
  
  		btScalar positionalError = 0.f;
  		btScalar velocityError = restitution - rel_vel;  // * damping;
  
  		if (penetration > 0)
  		{
  			positionalError = 0;
  
  			velocityError -= penetration * invTimeStep;
  		}
  		else
  		{
  			positionalError = -penetration * erp * invTimeStep;
  		}
  
  		btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
  		btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
  
  		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
  		{
  			//combine position and velocity into rhs
  			solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;  //-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
  			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 = cfm * solverConstraint.m_jacDiagABInv;
  		solverConstraint.m_lowerLimit = 0;
  		solverConstraint.m_upperLimit = 1e10f;
  	}
  }
  
  void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse(btSolverConstraint& solverConstraint,
  	int solverBodyIdA, int solverBodyIdB,
  	btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
  {
  	{
  		btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
  		
  		frictionConstraint1.m_appliedImpulse = 0.f;
  	}
  
  	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
  	{
  		btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
  		
  		frictionConstraint2.m_appliedImpulse = 0.f;
  	}
  }
  
  void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal)
  {
  	btCollisionObject *colObj0 = 0, *colObj1 = 0;
  
  	colObj0 = (btCollisionObject*)manifold->getBody0();
  	colObj1 = (btCollisionObject*)manifold->getBody1();
  
  	int solverBodyIdA = getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
  	int solverBodyIdB = getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
  
  	//	btRigidBody* bodyA = btRigidBody::upcast(colObj0);
  	//	btRigidBody* bodyB = btRigidBody::upcast(colObj1);
  
  	btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
  	btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
  
  	///avoid collision response between two static objects
  	if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
  		return;
  
  	int rollingFriction = 1;
  	for (int j = 0; j < manifold->getNumContacts(); j++)
  	{
  		btManifoldPoint& cp = manifold->getContactPoint(j);
  
  		if (cp.getDistance() <= manifold->getContactProcessingThreshold())
  		{
  			btVector3 rel_pos1;
  			btVector3 rel_pos2;
  			btScalar relaxation;
  
  			int frictionIndex = m_tmpSolverContactConstraintPool.size();
  			btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
  			solverConstraint.m_solverBodyIdA = solverBodyIdA;
  			solverConstraint.m_solverBodyIdB = solverBodyIdB;
  
  			solverConstraint.m_originalContactPoint = &cp;
  
  			const btVector3& pos1 = cp.getPositionWorldOnA();
  			const btVector3& pos2 = cp.getPositionWorldOnB();
  
  			rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
  			rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
  
  			btVector3 vel1;
  			btVector3 vel2;
  
  			solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1, vel1);
  			solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2, vel2);
  
  			btVector3 vel = vel1 - vel2;
  			btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
  
  			setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
  
  			/////setup the friction constraints
  
  			solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
  
  			if ((cp.m_combinedRollingFriction > 0.f) && (rollingFriction > 0))
  			{
  				{
  					addTorsionalFrictionConstraint(cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.m_combinedSpinningFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
  					btVector3 axis0, axis1;
  					btPlaneSpace1(cp.m_normalWorldOnB, axis0, axis1);
  					axis0.normalize();
  					axis1.normalize();
  
  					applyAnisotropicFriction(colObj0, axis0, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
  					applyAnisotropicFriction(colObj1, axis0, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
  					applyAnisotropicFriction(colObj0, axis1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
  					applyAnisotropicFriction(colObj1, axis1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
  					if (axis0.length() > 0.001)
  						addTorsionalFrictionConstraint(axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp,
  							cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
  					if (axis1.length() > 0.001)
  						addTorsionalFrictionConstraint(axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp,
  							cp.m_combinedRollingFriction, 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 SOLVER_USE_2_FRICTION_DIRECTIONS.
  			///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
  			///
  			///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
  			///
  			///The user can manually override the friction directions for certain contacts using a contact callback,
  			///and use contactPoint.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
  			///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 & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
  			{
  				cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
  				btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
  				if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
  				{
  					cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel);
  					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
  					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
  					addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
  
  					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
  					{
  						cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
  						cp.m_lateralFrictionDir2.normalize();  //??
  						applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
  						applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
  						addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
  					}
  				}
  				else
  				{
  					btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2);
  
  					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
  					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
  					addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
  
  					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
  					{
  						applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
  						applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
  						addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
  					}
  
  					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
  					{
  						cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
  					}
  				}
  			}
  			else
  			{
  				addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
  
  				if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
  					addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
  				}
  			setFrictionConstraintImpulse(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
  			}
  		}
  	}
  
  void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
  {
  	int i;
  	btPersistentManifold* manifold = 0;
  	//			btCollisionObject* colObj0=0,*colObj1=0;
  
  	for (i = 0; i < numManifolds; i++)
  	{
  		manifold = manifoldPtr[i];
  		convertContact(manifold, infoGlobal);
  	}
  }
  
  void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* currentConstraintRow,
  	btTypedConstraint* constraint,
  	const btTypedConstraint::btConstraintInfo1& info1,
  	int solverBodyIdA,
  	int solverBodyIdB,
  	const btContactSolverInfo& infoGlobal)
  {
  	const btRigidBody& rbA = constraint->getRigidBodyA();
  	const btRigidBody& rbB = constraint->getRigidBodyB();
  
  	const btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
  	const btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
  
  	int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
  	if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations)
  		m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
  
  	for (int j = 0; j < info1.m_numConstraintRows; j++)
  	{
  		memset(&currentConstraintRow[j], 0, sizeof(btSolverConstraint));
  		currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
  		currentConstraintRow[j].m_upperLimit = SIMD_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;
  	}
  
  	// these vectors are already cleared in initSolverBody, no need to redundantly clear again
  	btAssert(bodyAPtr->getDeltaLinearVelocity().isZero());
  	btAssert(bodyAPtr->getDeltaAngularVelocity().isZero());
  	btAssert(bodyAPtr->getPushVelocity().isZero());
  	btAssert(bodyAPtr->getTurnVelocity().isZero());
  	btAssert(bodyBPtr->getDeltaLinearVelocity().isZero());
  	btAssert(bodyBPtr->getDeltaAngularVelocity().isZero());
  	btAssert(bodyBPtr->getPushVelocity().isZero());
  	btAssert(bodyBPtr->getTurnVelocity().isZero());
  	//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);
  
  	btTypedConstraint::btConstraintInfo2 info2;
  	info2.fps = 1.f / infoGlobal.m_timeStep;
  	info2.erp = infoGlobal.m_erp;
  	info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
  	info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
  	info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
  	info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
  	info2.rowskip = sizeof(btSolverConstraint) / sizeof(btScalar);  //check this
  																	///the size of btSolverConstraint needs be a multiple of btScalar
  	btAssert(info2.rowskip * sizeof(btScalar) == sizeof(btSolverConstraint));
  	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;
  	constraint->getInfo2(&info2);
  
  	///finalize the constraint setup
  	for (int j = 0; j < info1.m_numConstraintRows; j++)
  	{
  		btSolverConstraint& solverConstraint = currentConstraintRow[j];
  
  		if (solverConstraint.m_upperLimit >= constraint->getBreakingImpulseThreshold())
  		{
  			solverConstraint.m_upperLimit = constraint->getBreakingImpulseThreshold();
  		}
  
  		if (solverConstraint.m_lowerLimit <= -constraint->getBreakingImpulseThreshold())
  		{
  			solverConstraint.m_lowerLimit = -constraint->getBreakingImpulseThreshold();
  		}
  
  		solverConstraint.m_originalContactPoint = constraint;
  
  		{
  			const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
  			solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld() * ftorqueAxis1 * constraint->getRigidBodyA().getAngularFactor();
  		}
  		{
  			const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
  			solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld() * ftorqueAxis2 * constraint->getRigidBodyB().getAngularFactor();
  		}
  
  		{
  			btVector3 iMJlA = solverConstraint.m_contactNormal1 * rbA.getInvMass();
  			btVector3 iMJaA = rbA.getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal;
  			btVector3 iMJlB = solverConstraint.m_contactNormal2 * rbB.getInvMass();  //sign of normal?
  			btVector3 iMJaB = rbB.getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal;
  
  			btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
  			sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
  			sum += iMJlB.dot(solverConstraint.m_contactNormal2);
  			sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
  			btScalar fsum = btFabs(sum);
  			btAssert(fsum > SIMD_EPSILON);
  			btScalar sorRelaxation = 1.f;  //todo: get from globalInfo?
  			solverConstraint.m_jacDiagABInv = fsum > SIMD_EPSILON ? sorRelaxation / sum : 0.f;
  		}
  
  		{
  			btScalar rel_vel;
  			btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0, 0, 0);
  			btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0, 0, 0);
  
  			btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0, 0, 0);
  			btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalTorqueImpulse : btVector3(0, 0, 0);
  
  			btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity() + externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity() + externalTorqueImpulseA);
  
  			btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity() + externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity() + externalTorqueImpulseB);
  
  			rel_vel = vel1Dotn + vel2Dotn;
  			btScalar restitution = 0.f;
  			btScalar positionalError = solverConstraint.m_rhs;  //already filled in by getConstraintInfo2
  			btScalar velocityError = restitution - rel_vel * info2.m_damping;
  			btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
  			btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
  			solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
  			solverConstraint.m_appliedImpulse = 0.f;
  		}
  	}
  }
  
  void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal)
  {
  	BT_PROFILE("convertJoints");
  	for (int j = 0; j < numConstraints; j++)
  	{
  		btTypedConstraint* constraint = constraints[j];
  		constraint->buildJacobian();
  		constraint->internalSetAppliedImpulse(0.0f);
  	}
  
  	int totalNumRows = 0;
  
  	m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
  	//calculate the total number of contraint rows
  	for (int i = 0; i < numConstraints; i++)
  	{
  		btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
  		btJointFeedback* 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())
  		{
  			constraints[i]->getInfo1(&info1);
  		}
  		else
  		{
  			info1.m_numConstraintRows = 0;
  			info1.nub = 0;
  		}
  		totalNumRows += info1.m_numConstraintRows;
  	}
  	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
  
  	///setup the btSolverConstraints
  	int currentRow = 0;
  
  	for (int i = 0; i < numConstraints; i++)
  	{
  		const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
  
  		if (info1.m_numConstraintRows)
  		{
  			btAssert(currentRow < totalNumRows);
  
  			btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
  			btTypedConstraint* constraint = constraints[i];
  			btRigidBody& rbA = constraint->getRigidBodyA();
  			btRigidBody& rbB = constraint->getRigidBodyB();
  
  			int solverBodyIdA = getOrInitSolverBody(rbA, infoGlobal.m_timeStep);
  			int solverBodyIdB = getOrInitSolverBody(rbB, infoGlobal.m_timeStep);
  
  			convertJoint(currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal);
  		}
  		currentRow += info1.m_numConstraintRows;
  	}
  }
  
  void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
  {
  	BT_PROFILE("convertBodies");
  	for (int i = 0; i < numBodies; i++)
  	{
  		bodies[i]->setCompanionId(-1);
  	}
  #if BT_THREADSAFE
  	m_kinematicBodyUniqueIdToSolverBodyTable.resize(0);
  #endif  // BT_THREADSAFE
  
  	m_tmpSolverBodyPool.reserve(numBodies + 1);
  	m_tmpSolverBodyPool.resize(0);
  
  	//btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
  	//initSolverBody(&fixedBody,0);
  
  	for (int i = 0; i < numBodies; i++)
  	{
  		int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
  
  		btRigidBody* body = btRigidBody::upcast(bodies[i]);
  		if (body && body->getInvMass())
  		{
  			btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
  			btVector3 gyroForce(0, 0, 0);
  			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
  			{
  				gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
  				solverBody.m_externalTorqueImpulse -= gyroForce * body->getInvInertiaTensorWorld() * infoGlobal.m_timeStep;
  			}
  			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
  			{
  				gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
  				solverBody.m_externalTorqueImpulse += gyroForce;
  			}
  			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
  			{
  				gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
  				solverBody.m_externalTorqueImpulse += gyroForce;
  			}
  		}
  	}
  }
  
  btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
  {
  	m_fixedBodyId = -1;
  	BT_PROFILE("solveGroupCacheFriendlySetup");
  	(void)debugDrawer;
  
  	// if solver mode has changed,
  	if (infoGlobal.m_solverMode != m_cachedSolverMode)
  	{
  		// update solver functions to use SIMD or non-SIMD
  		bool useSimd = !!(infoGlobal.m_solverMode & SOLVER_SIMD);
  		setupSolverFunctions(useSimd);
  		m_cachedSolverMode = infoGlobal.m_solverMode;
  	}
  	m_maxOverrideNumSolverIterations = 0;
  
  #ifdef BT_ADDITIONAL_DEBUG
  	//make sure that dynamic bodies exist for all (enabled) constraints
  	for (int i = 0; i < numConstraints; i++)
  	{
  		btTypedConstraint* constraint = constraints[i];
  		if (constraint->isEnabled())
  		{
  			if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
  			{
  				bool found = false;
  				for (int b = 0; b < numBodies; b++)
  				{
  					if (&constraint->getRigidBodyA() == bodies[b])
  					{
  						found = true;
  						break;
  					}
  				}
  				btAssert(found);
  			}
  			if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
  			{
  				bool found = false;
  				for (int b = 0; b < numBodies; b++)
  				{
  					if (&constraint->getRigidBodyB() == bodies[b])
  					{
  						found = true;
  						break;
  					}
  				}
  				btAssert(found);
  			}
  		}
  	}
  	//make sure that dynamic bodies exist for all contact manifolds
  	for (int i = 0; i < numManifolds; i++)
  	{
  		if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
  		{
  			bool found = false;
  			for (int b = 0; b < numBodies; b++)
  			{
  				if (manifoldPtr[i]->getBody0() == bodies[b])
  				{
  					found = true;
  					break;
  				}
  			}
  			btAssert(found);
  		}
  		if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
  		{
  			bool found = false;
  			for (int b = 0; b < numBodies; b++)
  			{
  				if (manifoldPtr[i]->getBody1() == bodies[b])
  				{
  					found = true;
  					break;
  				}
  			}
  			btAssert(found);
  		}
  	}
  #endif  //BT_ADDITIONAL_DEBUG
  
  	//convert all bodies
  	convertBodies(bodies, numBodies, infoGlobal);
  
  	convertJoints(constraints, numConstraints, infoGlobal);
  
  	convertContacts(manifoldPtr, numManifolds, infoGlobal);
  
  	//	btContactSolverInfo 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 & 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;
  }
  
  btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/)
  {
  	BT_PROFILE("solveSingleIteration");
  	btScalar leastSquaresResidual = 0.f;
  
  	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
  	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
  	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
  
  	if (infoGlobal.m_solverMode & 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 = btRandInt2(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 = btRandInt2(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 = btRandInt2(j + 1);
  					m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
  					m_orderFrictionConstraintPool[swapi] = tmp;
  				}
  			}
  		}
  	}
  
  	///solve all joint constraints
  	for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
  	{
  		btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
  		if (iteration < constraint.m_overrideNumSolverIterations)
  		{
  			btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint);
  			leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
  		}
  	}
  
  	if (iteration < infoGlobal.m_numIterations)
  	{
  		for (int j = 0; j < numConstraints; j++)
  		{
  			if (constraints[j]->isEnabled())
  			{
  				int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(), infoGlobal.m_timeStep);
  				int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(), infoGlobal.m_timeStep);
  				btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
  				btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
  				constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
  			}
  		}
  
  		///solve all contact constraints
  		if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
  		{
  			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
  			int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
  
  			for (int c = 0; c < numPoolConstraints; c++)
  			{
  				btScalar totalImpulse = 0;
  
  				{
  					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
  					btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
  					leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
  
  					totalImpulse = solveManifold.m_appliedImpulse;
  				}
  				bool applyFriction = true;
  				if (applyFriction)
  				{
  					{
  						btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]];
  
  						if (totalImpulse > btScalar(0))
  						{
  							solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
  							solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
  
  							btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
  							leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
  						}
  					}
  
  					if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
  					{
  						btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]];
  
  						if (totalImpulse > btScalar(0))
  						{
  							solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
  							solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
  
  							btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
  							leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
  						}
  					}
  				}
  			}
  		}
  		else  //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 btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
  				btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
  				leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
  			}
  
  			///solve all friction constraints
  
  			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
  			for (j = 0; j < numFrictionPoolConstraints; j++)
  			{
  				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
  				btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
  
  				if (totalImpulse > btScalar(0))
  				{
  					solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
  					solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
  
  					btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
  					leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
  				}
  			}
  		}
  
  		int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
  		for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
  		{
  			btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
  			btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
  			if (totalImpulse > btScalar(0))
  			{
  				btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
  				if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
  					rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
  
  				rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
  				rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
  
  				btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
  				leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
  			}
  		}
  	}
  	return leastSquaresResidual;
  }
  
  void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
  {
  	BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
  	int iteration;
  	if (infoGlobal.m_splitImpulse)
  	{
  		{
  			for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
  			{
  				btScalar leastSquaresResidual = 0.f;
  				{
  					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
  					int j;
  					for (j = 0; j < numPoolConstraints; j++)
  					{
  						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
  
  						btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
  						leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
  					}
  				}
  				if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
  				{
  #ifdef VERBOSE_RESIDUAL_PRINTF
  					printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
  #endif
  					break;
  				}
  			}
  		}
  	}
  }
  
  btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
  {
  	BT_PROFILE("solveGroupCacheFriendlyIterations");
  
  	{
  		///this is a special step to resolve penetrations (just for contacts)
  		solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
  
  		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--)
  		{
  			m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
  
  			if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
  			{
  #ifdef VERBOSE_RESIDUAL_PRINTF
  				printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
  #endif
  				m_analyticsData.m_numSolverCalls++;
  				m_analyticsData.m_numIterationsUsed = iteration+1;
  				m_analyticsData.m_islandId = -2;
  				if (numBodies>0)
  					m_analyticsData.m_islandId = bodies[0]->getCompanionId();
  				m_analyticsData.m_numBodies = numBodies;
  				m_analyticsData.m_numContactManifolds = numManifolds;
  				m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
  				break;
  			}
  		}
  	}
  	return 0.f;
  }
  
  void btSequentialImpulseConstraintSolver::writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
  {
  	for (int j = iBegin; j < iEnd; j++)
  	{
  		const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
  		btManifoldPoint* pt = (btManifoldPoint*)solveManifold.m_originalContactPoint;
  		btAssert(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 & SOLVER_USE_2_FRICTION_DIRECTIONS))
  		{
  			pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse;
  		}
  		//do a callback here?
  	}
  }
  
  void btSequentialImpulseConstraintSolver::writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
  {
  	for (int j = iBegin; j < iEnd; j++)
  	{
  		const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
  		btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
  		btJointFeedback* fb = constr->getJointFeedback();
  		if (fb)
  		{
  			fb->m_appliedForceBodyA += solverConstr.m_contactNormal1 * solverConstr.m_appliedImpulse * constr->getRigidBodyA().getLinearFactor() / infoGlobal.m_timeStep;
  			fb->m_appliedForceBodyB += solverConstr.m_contactNormal2 * solverConstr.m_appliedImpulse * constr->getRigidBodyB().getLinearFactor() / infoGlobal.m_timeStep;
  			fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal * constr->getRigidBodyA().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep;
  			fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal * constr->getRigidBodyB().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; /*RGM ???? */
  		}
  
  		constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
  		if (btFabs(solverConstr.m_appliedImpulse) >= constr->getBreakingImpulseThreshold())
  		{
  			constr->setEnabled(false);
  		}
  	}
  }
  
  void btSequentialImpulseConstraintSolver::writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
  {
  	for (int i = iBegin; i < iEnd; i++)
  	{
  		btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
  		if (body)
  		{
  			if (infoGlobal.m_splitImpulse)
  				m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
  			else
  				m_tmpSolverBodyPool[i].writebackVelocity();
  
  			m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
  				m_tmpSolverBodyPool[i].m_linearVelocity +
  				m_tmpSolverBodyPool[i].m_externalForceImpulse);
  
  			m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
  				m_tmpSolverBodyPool[i].m_angularVelocity +
  				m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
  
  			if (infoGlobal.m_splitImpulse)
  				m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
  
  			m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
  		}
  	}
  }
  
  btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
  {
  	BT_PROFILE("solveGroupCacheFriendlyFinish");
  
  	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
  	{
  		writeBackContacts(0, m_tmpSolverContactConstraintPool.size(), infoGlobal);
  	}
  
  	writeBackJoints(0, m_tmpSolverNonContactConstraintPool.size(), infoGlobal);
  	writeBackBodies(0, m_tmpSolverBodyPool.size(), infoGlobal);
  
  	m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
  	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
  	m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
  	m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
  
  	m_tmpSolverBodyPool.resizeNoInitialize(0);
  	return 0.f;
  }
  
  /// btSequentialImpulseConstraintSolver Sequentially applies impulses
  btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer, btDispatcher* /*dispatcher*/)
  {
  	BT_PROFILE("solveGroup");
  	//you need to provide at least some bodies
  
  	solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
  
  	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
  
  	solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
  
  	return 0.f;
  }
  
  void btSequentialImpulseConstraintSolver::reset()
  {
  	m_btSeed2 = 0;
  }
  