godot/thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp

/*
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;
}