prepare and added constraint solver optimizations, not activated yet.

src/BulletDynamics/ConstraintSolver/btConstraintSolver.h

@@ -20,10 +20,12 @@ subject to the following restrictions:
 
 class btPersistentManifold;
 class btRigidBody;
+class btCollisionObject;
 class btTypedConstraint;
 struct btContactSolverInfo;
 struct btBroadphaseProxy;
 class btIDebugDraw;
+class btStackAlloc;
 
 /// btConstraintSolver provides solver interface
 class btConstraintSolver
@@ -33,7 +35,7 @@ public:
 
 	virtual ~btConstraintSolver() {}
 	
-	virtual btScalar solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0;
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) = 0;
 
 };
 

src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp

@@ -89,12 +89,13 @@ btScalar resolveSingleCollision(
 	const btContactSolverInfo& solverInfo)
 {
 
-	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
-	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
+	const btVector3& pos1_ = contactPoint.getPositionWorldOnA();
+	const btVector3& pos2_ = contactPoint.getPositionWorldOnB();
    	const btVector3& normal = contactPoint.m_normalWorldOnB;
 
-	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
-	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	//constant over all iterations
+	btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition();
 	
 	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
 	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);

src/BulletDynamics/ConstraintSolver/btContactConstraint.h

@@ -110,4 +110,13 @@ btScalar resolveSingleFriction(
 	const btContactSolverInfo& solverInfo
 		);
 
+
+
+btScalar resolveSingleCollisionCombined(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo
+		);
+
 #endif //CONTACT_CONSTRAINT_H

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -25,6 +25,12 @@ subject to the following restrictions:
 #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"
 
 #ifdef USE_PROFILE
 #include "LinearMath/btQuickprof.h"
@@ -105,7 +111,7 @@ bool  MyContactDestroyedCallback(void* userPersistentData)
 
 
 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
-:m_solverMode(SOLVER_RANDMIZE_ORDER) //not using SOLVER_USE_WARMSTARTING
+:m_solverMode(SOLVER_RANDMIZE_ORDER)//SOLVER_USE_WARMSTARTING)//SOLVER_RANDMIZE_ORDER) //not using SOLVER_USE_WARMSTARTING
 {
 	gContactDestroyedCallback = &MyContactDestroyedCallback;
 
@@ -120,10 +126,579 @@ btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
 		}
 }
 
-/// btSequentialImpulseConstraintSolver Sequentially applies impulses
-btScalar btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+
+void	initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody)
+{
+	int size = sizeof(btSolverBody);
+	int sizeofrb = sizeof(btRigidBody);
+	int sizemanifold = sizeof(btPersistentManifold);
+	int sizeofmp = sizeof(btManifoldPoint);
+	int sizeofPersistData = sizeof (btConstraintPersistentData);
+
+	solverBody->m_angularVelocity = rigidbody->getAngularVelocity();
+	solverBody->m_centerOfMassPosition = rigidbody->getCenterOfMassPosition();
+	solverBody->m_friction = rigidbody->getFriction();
+	solverBody->m_invInertiaWorld = rigidbody->getInvInertiaTensorWorld();
+	solverBody->m_invMass = rigidbody->getInvMass();
+	solverBody->m_linearVelocity = rigidbody->getLinearVelocity();
+	solverBody->m_originalBody = rigidbody;
+}
+
+btScalar penetrationResolveFactor = btScalar(0.9);
+btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
+{
+	btScalar rest = restitution * -rel_vel;
+	return rest;
+}
+
+
+
+
+
+
+//velocity + friction
+//response  between two dynamic objects with friction
+SIMD_FORCE_INLINE btScalar resolveSingleCollisionCombinedCacheFriendly(
+	btSolverBody& body1,
+	btSolverBody& body2,
+	btSolverConstraint& contactConstraint,
+	const btContactSolverInfo& solverInfo)
+{
+	btScalar normalImpulse(0.f);
+	{
+		if (contactConstraint.m_penetration < 0.f)
+			return 0.f;
+
+	//  Optimized version of projected relative velocity, use precomputed cross products with normal
+	//	body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
+	//	body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
+	//	btVector3 vel = vel1 - vel2;
+	//	btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
+
+		btScalar rel_vel;
+		btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
+					+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
+		btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
+					+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
+
+		rel_vel = vel1Dotn-vel2Dotn;
+
+
+		btScalar positionalError = contactConstraint.m_penetration;
+		btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
+
+		btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
+		btScalar	velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
+		btScalar normalImpulse = penetrationImpulse+velocityImpulse;
+		
+		// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+		btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
+		btScalar sum = oldNormalImpulse + normalImpulse;
+		contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
+
+		btScalar oldVelocityImpulse = contactConstraint.m_appliedVelocityImpulse;
+		btScalar velocitySum = oldVelocityImpulse + velocityImpulse;
+		contactConstraint.m_appliedVelocityImpulse = btScalar(0.) > velocitySum ? btScalar(0.): velocitySum;
+
+		normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
+
+		if (body1.m_invMass)
+		{
+			body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
+				contactConstraint.m_angularComponentA,normalImpulse);
+		}
+		if (body2.m_invMass)
+		{
+			body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
+				contactConstraint.m_angularComponentB,-normalImpulse);
+		}
+
+	}
+
+	
+
+	return normalImpulse;
+}
+
+
+#ifndef NO_FRICTION_TANGENTIALS
+
+SIMD_FORCE_INLINE btScalar resolveSingleFrictionCacheFriendly(
+	btSolverBody& body1,
+	btSolverBody& body2,
+	btSolverConstraint& contactConstraint,
+	const btContactSolverInfo& solverInfo,
+	btScalar appliedNormalImpulse)
 {
 
+	
+	btScalar combinedFriction = contactConstraint.m_friction;
+	
+	btScalar limit = appliedNormalImpulse * combinedFriction;
+	
+	if (appliedNormalImpulse>btScalar(0.))
+	//friction
+	{
+		
+		btScalar j1;
+		{
+
+			btScalar rel_vel;
+			btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
+						+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
+			btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
+				+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
+			rel_vel = vel1Dotn-vel2Dotn;
+
+			// calculate j that moves us to zero relative velocity
+			j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
+			btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
+			contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
+			GEN_set_min(contactConstraint.m_appliedImpulse, limit);
+			GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
+			j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
+
+		}
+	
+		if (body1.m_invMass)
+		{
+			body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
+		}
+		if (body2.m_invMass)
+		{
+			body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
+		}
+
+	} 
+	return 0.f;
+}
+
+
+#else
+
+//velocity + friction
+//response  between two dynamic objects with friction
+btScalar resolveSingleFrictionCacheFriendly(
+	btSolverBody& body1,
+	btSolverBody& body2,
+	btSolverConstraint& contactConstraint,
+	const btContactSolverInfo& solverInfo)
+{
+
+	btVector3 vel1;
+	btVector3 vel2;
+	btScalar normalImpulse(0.f);
+
+	{
+		const btVector3& normal = contactConstraint.m_contactNormal;
+		if (contactConstraint.m_penetration < 0.f)
+			return 0.f;
+
+
+		body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
+		body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
+		btVector3 vel = vel1 - vel2;
+		btScalar rel_vel;
+		rel_vel = normal.dot(vel);
+
+		btVector3 lat_vel = vel - normal * rel_vel;
+		btScalar lat_rel_vel = lat_vel.length2();
+
+		btScalar combinedFriction = contactConstraint.m_friction;
+		const btVector3& rel_pos1 = contactConstraint.m_rel_posA;
+		const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
+
+
+		//if (contactConstraint.m_appliedVelocityImpulse > 0.f)
+		if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
+		{
+			lat_rel_vel = btSqrt(lat_rel_vel);
+
+			lat_vel /= lat_rel_vel;
+			btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel);
+			btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
+			btScalar friction_impulse = lat_rel_vel /
+				(body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
+			btScalar normal_impulse = contactConstraint.m_appliedVelocityImpulse * combinedFriction;
+
+			GEN_set_min(friction_impulse, normal_impulse);
+			GEN_set_max(friction_impulse, -normal_impulse);
+			body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
+			body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
+		}
+	}
+
+	return normalImpulse;
+}
+
+#endif //NO_FRICTION_TANGENTIALS
+
+btAlignedObjectArray<btSolverBody>	tmpSolverBodyPool;
+btAlignedObjectArray<btSolverConstraint>	tmpSolverConstraintPool;
+btAlignedObjectArray<btSolverConstraint>	tmpSolverFrictionConstraintPool;
+
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+{
+
+	if (!(numConstraints + numManifolds))
+		return 0.f;
+
+	BEGIN_PROFILE("gatherSolverData");
+
+	
+
+	int sizeofSB = sizeof(btSolverBody);
+	int sizeofSC = sizeof(btSolverConstraint);
+
+
+	if (numManifolds)
+	{
+		//if m_stackAlloc, try to pack bodies/constraints to speed up solving
+//		btBlock*					sablock;
+//		sablock = stackAlloc->beginBlock();
+
+	//	int memsize = 16;
+//		unsigned char* stackMemory = stackAlloc->allocate(memsize);
+
+		
+		tmpSolverBodyPool.reserve(numManifolds*2);
+
+		{
+			for (int i=0;i<numBodies;i++)
+			{
+				btRigidBody* rb = btRigidBody::upcast(bodies[i]);
+				if (rb && 	(rb->getIslandTag() >= 0))
+				{
+					btAssert(rb->getCompanionId() < 0);
+					int solverBodyId = tmpSolverBodyPool.size();
+					btSolverBody& solverBody = tmpSolverBodyPool.expand();
+					initSolverBody(&solverBody,rb);
+					rb->setCompanionId(solverBodyId);
+				} 
+			}
+		}
+
+		
+		tmpSolverConstraintPool.reserve(numManifolds*4+numConstraints);
+		tmpSolverFrictionConstraintPool.reserve(numManifolds*4);
+
+		{
+			int i;
+			for (i=0;i<numManifolds;i++)
+			{
+				btPersistentManifold* manifold = manifoldPtr[i];
+				btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
+				btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();
+
+				manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
+		
+				int solverBodyIdA;
+				int solverBodyIdB;
+
+				if (manifold->getNumContacts())
+				{
+
+					
+
+					if (rb0->getIslandTag() >= 0)
+					{
+						solverBodyIdA = rb0->getCompanionId();
+					} else
+					{
+						//create a static body
+						solverBodyIdA = tmpSolverBodyPool.size();
+						btSolverBody& solverBody = tmpSolverBodyPool.expand();
+						initSolverBody(&solverBody,rb0);
+					}
+
+					if (rb1->getIslandTag() >= 0)
+					{
+						solverBodyIdB = rb1->getCompanionId();
+					} else
+					{
+						//create a static body
+						solverBodyIdB = tmpSolverBodyPool.size();
+						btSolverBody& solverBody = tmpSolverBodyPool.expand();
+						initSolverBody(&solverBody,rb1);
+					}
+				}
+
+				for (int j=0;j<manifold->getNumContacts();j++)
+				{
+					
+					btManifoldPoint& cp = manifold->getContactPoint(j);
+
+					int frictionIndex = tmpSolverConstraintPool.size();
+
+					if (cp.getDistance() <= btScalar(0.))
+					{
+						
+						const btVector3& pos1 = cp.getPositionWorldOnA();
+						const btVector3& pos2 = cp.getPositionWorldOnB();
+
+						btVector3 rel_pos1 = pos1 - rb0->getCenterOfMassPosition(); 
+						btVector3 rel_pos2 = pos2 - rb1->getCenterOfMassPosition();
+
+						
+							btScalar relaxation = 1.f;
+
+						{
+							btSolverConstraint& solverConstraint = tmpSolverConstraintPool.expand();
+
+							solverConstraint.m_solverBodyIdA = solverBodyIdA;
+							solverConstraint.m_solverBodyIdB = solverBodyIdB;
+							solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
+
+					
+
+							{
+								//can be optimized, the cross products are already calculated
+								btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
+								btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
+								btScalar denom = relaxation/(denom0+denom1);
+								solverConstraint.m_jacDiagABInv = denom;
+							}
+
+							solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
+							solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
+							solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
+
+
+							btVector3 vel1 = rb0->getVelocityInLocalPoint(rel_pos1);
+							btVector3 vel2 = rb1->getVelocityInLocalPoint(rel_pos2);
+				
+							btVector3 vel = vel1 - vel2;
+							btScalar rel_vel;
+							rel_vel = cp.m_normalWorldOnB.dot(vel);
+							
+
+							solverConstraint.m_penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations);
+							solverConstraint.m_friction = cp.m_combinedFriction;
+							btScalar rest =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
+							if (rest <= btScalar(0.))
+							{
+								rest = 0.f;
+							};
+							
+							btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
+							if (rest > penVel)
+							{
+								rest = btScalar(0.);
+							}
+							solverConstraint.m_restitution = rest;
+
+							solverConstraint.m_penetration *= -(infoGlobal.m_erp/infoGlobal.m_timeStep);
+
+							solverConstraint.m_appliedImpulse = 0.f;
+							solverConstraint.m_appliedVelocityImpulse = 0.f;
+							
+					
+							btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+							solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*torqueAxis0;
+							btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
+							solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*torqueAxis1;
+						}
+
+						//create 2 '1d axis' constraints for 2 tangential friction directions
+			
+						//re-calculate friction direction every frame, todo: check if this is really needed
+						btVector3 frictionTangential0a, frictionTangential1b;
+
+						btPlaneSpace1(cp.m_normalWorldOnB,frictionTangential0a,frictionTangential1b);
+
+						{
+							btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand();
+							solverConstraint.m_contactNormal = frictionTangential0a;
+
+							solverConstraint.m_solverBodyIdA = solverBodyIdA;
+							solverConstraint.m_solverBodyIdB = solverBodyIdB;
+							solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
+							solverConstraint.m_frictionIndex = frictionIndex;
+
+							solverConstraint.m_friction = cp.m_combinedFriction;
+
+							solverConstraint.m_appliedImpulse = btScalar(0.);
+							solverConstraint.m_appliedVelocityImpulse = 0.f;
+
+							btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
+							btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
+							btScalar denom = relaxation/(denom0+denom1);
+							solverConstraint.m_jacDiagABInv = denom;
+
+							{
+							btVector3 ftorqueAxis0 = rel_pos1.cross(solverConstraint.m_contactNormal);
+							solverConstraint.m_relpos1CrossNormal = ftorqueAxis0;
+							solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis0;
+							}
+							{
+							btVector3 ftorqueAxis0 = rel_pos2.cross(solverConstraint.m_contactNormal);
+							solverConstraint.m_relpos2CrossNormal = ftorqueAxis0;
+							solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis0;
+							}
+
+						}
+
+
+						{
+
+							btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand();
+							solverConstraint.m_contactNormal = frictionTangential1b;
+
+							solverConstraint.m_solverBodyIdA = solverBodyIdA;
+							solverConstraint.m_solverBodyIdB = solverBodyIdB;
+							solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
+							solverConstraint.m_frictionIndex = frictionIndex;
+
+							solverConstraint.m_friction = cp.m_combinedFriction;
+
+							solverConstraint.m_appliedImpulse = btScalar(0.);
+							solverConstraint.m_appliedVelocityImpulse = 0.f;
+						
+							btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
+							btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
+							btScalar denom = relaxation/(denom0+denom1);
+							solverConstraint.m_jacDiagABInv = denom;
+							{
+								btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
+								solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
+								solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis1;
+								}
+							{
+								btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
+								solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
+								solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis1;
+								}
+							}
+
+					}
+				}
+			}
+		}
+	}
+	END_PROFILE("gatherSolverData");
+
+	BEGIN_PROFILE("prepareConstraints");
+
+	btContactSolverInfo info = infoGlobal;
+
+	{
+		int j;
+		for (j=0;j<numConstraints;j++)
+		{
+			btTypedConstraint* constraint = constraints[j];
+			constraint->buildJacobian();
+		}
+	}
+	
+	END_PROFILE("prepareConstraints");
+
+
+	BEGIN_PROFILE("solveConstraints");
+
+	//should traverse the contacts random order...
+	int iteration;
+	int j;
+	{
+		for ( iteration = 0;iteration<info.m_numIterations;iteration++)
+		{			
+			for (j=0;j<numConstraints;j++)
+			{
+				btTypedConstraint* constraint = constraints[j];
+				constraint->solveConstraint(info.m_timeStep);
+			}
+
+			{
+				int numPoolConstraints = tmpSolverConstraintPool.size();
+				for (j=0;j<numPoolConstraints;j++)
+				{
+					btSolverConstraint& solveManifold = tmpSolverConstraintPool[j];
+					switch (solveManifold.m_constraintType)
+					{
+					case btSolverConstraint::BT_SOLVER_CONTACT_1D:
+						{
+								resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
+									tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info);
+							break;
+						}
+					case btSolverConstraint::BT_SOLVER_FRICTION_1D:
+						{
+
+							break;
+						}
+
+					default:
+						{
+							//undefined constraint type?
+							btAssert(0);
+						}
+					};
+				}
+			}
+
+			{
+				 int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size();
+				for (j=0;j<numFrictionPoolConstraints;j++)
+				{
+					btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[j];
+					switch (solveManifold.m_constraintType)
+					{
+					case btSolverConstraint::BT_SOLVER_CONTACT_1D:
+						{
+							break;
+						}
+					case btSolverConstraint::BT_SOLVER_FRICTION_1D:
+						{
+
+							btScalar appliedNormalImpulse = tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+							resolveSingleFrictionCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
+								tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info,appliedNormalImpulse);
+							break;
+						}
+
+					default:
+						{
+							//undefined constraint type?
+							btAssert(0);
+						}
+					};
+				}
+			}
+			
+
+
+		}
+	}
+		
+	for (int i=0;i<tmpSolverBodyPool.size();i++)
+	{
+		tmpSolverBodyPool[i].writebackVelocity();
+	}
+
+	END_PROFILE("solveConstraints");
+
+
+	tmpSolverBodyPool.resize(0);
+	tmpSolverConstraintPool.resize(0);
+	tmpSolverFrictionConstraintPool.resize(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,btStackAlloc* stackAlloc)
+{
+
+
+
+	//return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
+
+
+
+	BEGIN_PROFILE("prepareConstraints");
+	
 
 	btContactSolverInfo info = infoGlobal;
 
@@ -160,11 +735,16 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold**
 		}
 	}
 	
+	END_PROFILE("prepareConstraints");
+
+
+	BEGIN_PROFILE("solveConstraints");
+
 	//should traverse the contacts random order...
 	int iteration;
 
 	{
-		for ( iteration = 0;iteration<numiter-1;iteration++)
+		for ( iteration = 0;iteration<numiter;iteration++)
 		{
 			int j;
 			if (m_solverMode & SOLVER_RANDMIZE_ORDER)
@@ -202,22 +782,23 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold**
 		}
 	}
 		
+	END_PROFILE("solveConstraints");
+
+
 #ifdef USE_PROFILE
 	btProfiler::endBlock("solve");
 #endif //USE_PROFILE
 
+
+
+
 	return btScalar(0.);
 }
 
 
 
 
-btScalar penetrationResolveFactor = btScalar(0.9);
-btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
-{
-	btScalar rest = restitution * -rel_vel;
-	return rest;
-}
+
 
 
 void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
@@ -304,10 +885,10 @@ void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
 				
 				btScalar combinedRestitution = cp.m_combinedRestitution;
 				
-				cpd->m_penetration = cp.getDistance();
+				cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
 				cpd->m_friction = cp.m_combinedFriction;
 				cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
-				if (cpd->m_restitution <= 0.) //btScalar(0.))
+				if (cpd->m_restitution <= btScalar(0.))
 				{
 					cpd->m_restitution = btScalar(0.0);
 
@@ -404,6 +985,44 @@ void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
 	}
 }
 
+
+btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
+{
+	btScalar maxImpulse = btScalar(0.);
+
+	{
+
+		btVector3 color(0,1,0);
+		{
+			if (cp.getDistance() <= btScalar(0.))
+			{
+
+				if (iter == 0)
+				{
+					if (debugDrawer)
+						debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
+				}
+
+				{
+
+					btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
+					btScalar impulse = resolveSingleCollisionCombined(
+						*body0,*body1,
+						cp,
+						info);
+
+					if (maxImpulse < impulse)
+						maxImpulse  = impulse;
+
+				}
+			}
+		}
+	}
+	return maxImpulse;
+}
+
+
+
 btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
 {
 

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -67,7 +67,12 @@ public:
 
 	virtual ~btSequentialImpulseConstraintSolver() {}
 	
-	virtual btScalar solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc);
+
+	virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+
+	btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
+
 
 	void	setSolverMode(int mode)
 	{

src/BulletDynamics/ConstraintSolver/btsolverbody.h

@@ -0,0 +1,71 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_SOLVER_BODY_H
+#define BT_SOLVER_BODY_H
+
+class	btRigidBody;
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+
+
+
+
+ATTRIBUTE_ALIGNED16 (struct	btSolverBody)
+{
+	btMatrix3x3		m_invInertiaWorld;
+	btVector3		m_centerOfMassPosition;
+	btVector3		m_linearVelocity;
+	btVector3		m_angularVelocity;
+	btRigidBody*	m_originalBody;
+	float			m_invMass;
+	float			m_friction;
+	float			m_unused;
+	btVector3		m_unused2;
+
+	inline void	getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
+	{
+		velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
+	}
+
+	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+	inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	{	
+		m_linearVelocity += linearComponent*impulseMagnitude;
+		m_angularVelocity += angularComponent*impulseMagnitude;
+	}
+
+	void	writebackVelocity()
+	{
+		if (m_invMass)
+		{
+			m_originalBody->setLinearVelocity(m_linearVelocity);
+			m_originalBody->setAngularVelocity(m_angularVelocity);
+		}
+	}
+
+	inline void	applyImpulse(const btVector3& impulse,const btVector3& rel_pos)
+	{
+		if (m_invMass)
+		{
+			m_linearVelocity += impulse * m_invMass;
+			btVector3 torqueImpulse = rel_pos.cross(impulse);
+			m_angularVelocity += m_invInertiaWorld * torqueImpulse;
+		}
+	}
+
+};
+
+#endif //BT_SOLVER_BODY_H

src/BulletDynamics/ConstraintSolver/btsolverconstraint.h

@@ -0,0 +1,63 @@
+
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_SOLVER_CONSTRAINT_H
+#define BT_SOLVER_CONSTRAINT_H
+
+class	btRigidBody;
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+
+//#define NO_FRICTION_TANGENTIALS 1
+
+///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
+ATTRIBUTE_ALIGNED16 (struct	btSolverConstraint)
+{
+	btVector3	m_relpos1CrossNormal;
+	btVector3	m_relpos2CrossNormal;
+	btVector3	m_contactNormal;
+	btVector3	m_angularComponentA;
+	btVector3	m_angularComponentB;
+
+	btScalar	m_appliedVelocityImpulse;
+	int			m_solverBodyIdA;
+	int			m_solverBodyIdB;
+	btScalar	m_friction;
+	btScalar	m_restitution;
+	btScalar	m_jacDiagABInv;
+	btScalar	m_penetration;
+	btScalar	m_appliedImpulse;
+
+	int			m_constraintType;
+	int			m_frictionIndex;
+	int			m_unusedPadding[2];
+
+	enum		btSolverConstraintType
+	{
+		BT_SOLVER_CONTACT_1D = 0,
+		BT_SOLVER_FRICTION_1D
+	};
+};
+
+
+
+
+
+
+#endif //BT_SOLVER_CONSTRAINT_H
+
+