prepare and added constraint solver optimizations, not activated yet.

Extras/quickstep/OdeConstraintSolver.cpp

@@ -26,6 +26,7 @@ subject to the following restrictions:
 #include "OdeContactJoint.h"
 #include "OdeSolverBody.h"
 #include <new.h>
+#include "LinearMath/btQuickprof.h"
 
 #include "LinearMath/btIDebugDraw.h"
 
@@ -71,8 +72,10 @@ m_erp(0.4f)
 
 
 //iterative lcp and penalty method
-btScalar OdeConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+btScalar OdeConstraintSolver::solveGroup(btCollisionObject** bodies,int numBulletBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
 {
+	BEGIN_PROFILE("prepareConstraints");
+
 	m_CurBody = 0;
 	m_CurJoint = 0;
 
@@ -95,6 +98,8 @@ btScalar OdeConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int
 		}
 	}
 
+	END_PROFILE("prepareConstraints");
+	BEGIN_PROFILE("solveConstraints");
 	SolveInternal1(m_cfm,m_erp,odeBodies,numBodies,joints,numJoints,infoGlobal);
 
 	//write back resulting velocities
@@ -106,6 +111,7 @@ btScalar OdeConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int
 			odeBodies[i]->m_originalBody->setAngularVelocity(odeBodies[i]->m_angularVelocity);
 		}
 	}
+	END_PROFILE("solveConstraints");
 	return 0.f;
 
 }

Extras/quickstep/OdeConstraintSolver.h

@@ -45,7 +45,7 @@ public:
 
 	virtual ~OdeConstraintSolver() {}
 	
-	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);
 
 	///setConstraintForceMixing, the cfm adds some positive value to the main diagonal
 	///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy'

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -54,6 +54,8 @@ protected:
 	int				m_collisionFlags;
 
 	int				m_islandTag1;
+	int				m_companionId;
+
 	int				m_activationState1;
 	btScalar			m_deactivationTime;
 
@@ -251,6 +253,16 @@ public:
 		m_islandTag1 = tag;
 	}
 
+	const int getCompanionId() const
+	{
+		return	m_companionId;
+	}
+
+	void	setCompanionId(int id)
+	{
+		m_companionId = id;
+	}
+
 	const btScalar			getHitFraction() const
 	{
 		return m_hitFraction; 

src/BulletCollision/CollisionDispatch/btManifoldResult.cpp

@@ -63,16 +63,23 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
 
 	bool isSwapped = m_manifoldPtr->getBody0() != m_body0;
 
-	btTransform transAInv = isSwapped? m_rootTransB.inverse() : m_rootTransA.inverse();
-	btTransform transBInv = isSwapped? m_rootTransA.inverse() : m_rootTransB.inverse();
-
 	btVector3 pointA = pointInWorld + normalOnBInWorld * depth;
-	btVector3 localA = transAInv(pointA );
+
-	btVector3 localB = transBInv(pointInWorld);
+	btVector3 localA;
+	btVector3 localB;
+	
+	if (isSwapped)
+	{
+		localA = m_rootTransB.invXform(pointA );
+		localB = m_rootTransA.invXform(pointInWorld);
+	} else
+	{
+		localA = m_rootTransA.invXform(pointA );
+		localB = m_rootTransB.invXform(pointInWorld);
+	}
+
 	btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
 
-	
-
 	int insertIndex = m_manifoldPtr->getCacheEntry(newPt);
 
 	newPt.m_combinedFriction = calculateCombinedFriction(m_body0,m_body1);

src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

@@ -8,7 +8,7 @@
 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
 
 #include <stdio.h>
-
+#include "LinearMath/btQuickprof.h"
 
 btSimulationIslandManager::btSimulationIslandManager()
 {
@@ -63,6 +63,7 @@ void	btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld
 		{
 			btCollisionObject*	collisionObject= colWorld->getCollisionObjectArray()[i];
 			collisionObject->setIslandTag(index);
+			collisionObject->setCompanionId(-1);
 			collisionObject->setHitFraction(btScalar(1.));
 			index++;
 			
@@ -93,9 +94,11 @@ void	btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col
 			if (collisionObject->mergesSimulationIslands())
 			{
 				collisionObject->setIslandTag( m_unionFind.find(index) );
+				collisionObject->setCompanionId(-1);
 			} else
 			{
 				collisionObject->setIslandTag(-1);
+				collisionObject->setCompanionId(-2);
 			}
 			index++;
 		}
@@ -137,6 +140,11 @@ class btPersistentManifoldSortPredicate
 //
 void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback)
 {
+
+	
+	BEGIN_PROFILE("islandUnionFindAndHeapSort");
+
+	
 	//we are going to sort the unionfind array, and store the element id in the size
 	//afterwards, we clean unionfind, to make sure no-one uses it anymore
 	
@@ -278,6 +286,10 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 
 	//int islandId;
 
+	END_PROFILE("islandUnionFindAndHeapSort");
+
+	btAlignedObjectArray<btCollisionObject*>	islandBodies;
+
 
 	//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
 	for (int startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
@@ -291,6 +303,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
                 {
                         int i = getUnionFind().getElement(endIslandIndex).m_sz;
                         btCollisionObject* colObj0 = collisionObjects[i];
+						islandBodies.push_back(colObj0);
                         if (!colObj0->isActive())
                                 islandSleeping = true;
                 }
@@ -319,12 +332,16 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 
 		if (!islandSleeping)
 		{
-			callback->ProcessIsland(startManifold,numIslandManifolds, islandId);
+			callback->ProcessIsland(&islandBodies[0],islandBodies.size(),startManifold,numIslandManifolds, islandId);
 		}
 		
 		if (numIslandManifolds)
 		{
 			startManifoldIndex = endManifoldIndex;
 		}
+
+		islandBodies.resize(0);
 	}
+
+	
 }

src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h

@@ -19,6 +19,7 @@ subject to the following restrictions:
 #include "../CollisionDispatch/btUnionFind.h"
 #include "btCollisionCreateFunc.h"
 
+class btCollisionObject;
 class btCollisionWorld;
 class btDispatcher;
 
@@ -49,7 +50,7 @@ public:
 	{
 		virtual ~IslandCallback() {};
 
-		virtual	void	ProcessIsland(class btPersistentManifold**	manifolds,int numManifolds, int islandId) = 0;
+		virtual	void	ProcessIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold**	manifolds,int numManifolds, int islandId) = 0;
 	};
 
 	void	buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback);

src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

@@ -29,7 +29,8 @@ class btManifoldPoint
 	{
 		public:
 			btManifoldPoint()
-				:m_userPersistentData(0)
+				:m_userPersistentData(0),
+				m_lifeTime(0)
 			{
 			}
 
@@ -76,12 +77,12 @@ class btManifoldPoint
 				return m_lifeTime;
 			}
 
-			btVector3 getPositionWorldOnA() {
+			const btVector3& getPositionWorldOnA() const {
 				return m_positionWorldOnA;
 //				return m_positionWorldOnB + m_normalWorldOnB * m_distance1;
 			}
 
-			const btVector3& getPositionWorldOnB()
+			const btVector3& getPositionWorldOnB() const
 			{
 				return m_positionWorldOnB;
 			}

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

@@ -115,11 +115,23 @@ public:
 	}
 	void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex)
 	{
-		assert(validContactDistance(newPoint));
+		btAssert(validContactDistance(newPoint));
 
-		clearUserCache(m_pointCache[insertIndex]);
+#define MAINTAIN_PERSISTENCY 1
+#ifdef MAINTAIN_PERSISTENCY
+		int	lifeTime = m_pointCache[insertIndex].getLifeTime();
+		btAssert(lifeTime>=0);
+		void* cache = m_pointCache[insertIndex].m_userPersistentData;
 		
 		m_pointCache[insertIndex] = newPoint;
+
+		m_pointCache[insertIndex].m_userPersistentData = cache;
+		m_pointCache[insertIndex].m_lifeTime = lifeTime;
+#else
+		clearUserCache(m_pointCache[insertIndex]);
+		m_pointCache[insertIndex] = newPoint;
+	
+#endif
 	}
 
 	bool validContactDistance(const btManifoldPoint& pt) const

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& pos1_ = contactPoint.getPositionWorldOnA();
-	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
+	const btVector3& pos2_ = contactPoint.getPositionWorldOnB();
    	const btVector3& normal = contactPoint.m_normalWorldOnB;
 
-	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	//constant over all iterations
-	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	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,9 +126,578 @@ 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
+
+

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -421,8 +421,6 @@ class btSortConstraintOnIslandPredicate
 void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
 	
-	BEGIN_PROFILE("solveConstraints");
-
 	struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
 	{
 
@@ -431,7 +429,7 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 		btTypedConstraint**		m_sortedConstraints;
 		int						m_numConstraints;
 		btIDebugDraw*			m_debugDrawer;
-
+		btStackAlloc*			m_stackAlloc;
 
 
 		InplaceSolverIslandCallback(
@@ -439,17 +437,19 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 			btConstraintSolver*	solver,
 			btTypedConstraint** sortedConstraints,
 			int	numConstraints,
-			btIDebugDraw*	debugDrawer)
+			btIDebugDraw*	debugDrawer,
+			btStackAlloc*			stackAlloc)
 			:m_solverInfo(solverInfo),
 			m_solver(solver),
 			m_sortedConstraints(sortedConstraints),
 			m_numConstraints(numConstraints),
-			m_debugDrawer(debugDrawer)
+			m_debugDrawer(debugDrawer),
+			m_stackAlloc(stackAlloc)
 		{
 
 		}
 
-		virtual	void	ProcessIsland(btPersistentManifold**	manifolds,int numManifolds, int islandId)
+		virtual	void	ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
 		{
 			//also add all non-contact constraints/joints for this island
 			btTypedConstraint** startConstraint = 0;
@@ -474,12 +474,11 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 				}
 			}
 
-			m_solver->solveGroup( manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer);
+			m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc);
 		}
 
 	};
 
-
 	//sorted version of all btTypedConstraint, based on islandId
 	btAlignedObjectArray<btTypedConstraint*>	sortedConstraints;
 	sortedConstraints.resize( m_constraints.size());
@@ -497,14 +496,13 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 	
 	btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
 	
-	InplaceSolverIslandCallback	solverCallback(	solverInfo,	m_constraintSolver, constraintsPtr,sortedConstraints.size(),	m_debugDrawer);
+	InplaceSolverIslandCallback	solverCallback(	solverInfo,	m_constraintSolver, constraintsPtr,sortedConstraints.size(),	m_debugDrawer,m_stackAlloc);
 
 	
 	
 	/// solve all the constraints for this island
 	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback);
 
-	END_PROFILE("solveConstraints");
 
 }
 

src/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -110,6 +110,8 @@ btRigidBody::btRigidBody( btScalar mass,const btTransform& worldTransform,btColl
 #endif //OBSOLETE_MOTIONSTATE_LESS
 
 
+
+
 //#define EXPERIMENTAL_JITTER_REMOVAL 1
 #ifdef EXPERIMENTAL_JITTER_REMOVAL
 //Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate
@@ -119,7 +121,7 @@ btScalar gClippedAngvelThresholdSqr = btScalar(0.01);
 btScalar	gClippedLinearThresholdSqr = btScalar(0.01);
 #endif //EXPERIMENTAL_JITTER_REMOVAL
 
-btScalar	gJitterVelocityDampingFactor = btScalar(1.);
+btScalar	gJitterVelocityDampingFactor = btScalar(0.9);
 
 void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
 {

src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp

@@ -71,7 +71,7 @@ int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b
 		btContactSolverInfo infoGlobal;
 		infoGlobal.m_timeStep = timeStep;
 		
-		m_constraintSolver->solveGroup(manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer);
+		m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc);
 	}
 
 	///integrate transforms
@@ -89,7 +89,7 @@ int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b
 void	btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
 {
 	m_gravity = gravity;
-	for (unsigned int i=0;i<m_collisionObjects.size();i++)
+	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -118,7 +118,7 @@ void	btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
 void	btSimpleDynamicsWorld::updateAabbs()
 {
 	btTransform predictedTrans;
-	for (unsigned int i=0;i<m_collisionObjects.size();i++)
+	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -138,7 +138,7 @@ void	btSimpleDynamicsWorld::updateAabbs()
 void	btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
 	btTransform predictedTrans;
-	for (unsigned int i=0;i<m_collisionObjects.size();i++)
+	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -157,7 +157,7 @@ void	btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
 
 void	btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 {
-	for (unsigned int i=0;i<m_collisionObjects.size();i++)
+	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);
@@ -180,7 +180,7 @@ void	btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 void	btSimpleDynamicsWorld::synchronizeMotionStates()
 {
 	//todo: iterate over awake simulation islands!
-	for (unsigned int i=0;i<m_collisionObjects.size();i++)
+	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
 		btRigidBody* body = btRigidBody::upcast(colObj);

src/LinearMath/btAlignedObjectArray.h

@@ -136,6 +136,17 @@ class btAlignedObjectArray
 		}
 	
 
+		SIMD_FORCE_INLINE	T&  expand()
+		{	
+			int sz = size();
+			if( sz == capacity() )
+			{
+				reserve( allocSize(size()) );
+			}
+			m_size++;
+
+			return m_data[sz];		
+		}
 
 		SIMD_FORCE_INLINE	void push_back(const T& _Val)
 		{	

src/LinearMath/btQuickprof.cpp

@@ -27,7 +27,7 @@ subject to the following restrictions:
 // Note: We must declare these private static variables again here to 
 // avoid link errors.
 bool btProfiler::mEnabled = false;
-hidden::Clock btProfiler::mClock;
+btClock btProfiler::mClock;
 unsigned long int btProfiler::mCurrentCycleStartMicroseconds = 0;
 unsigned long int btProfiler::mLastCycleDurationMicroseconds = 0;
 std::map<std::string, hidden::ProfileBlock*> btProfiler::mProfileBlocks;

src/LinearMath/btQuickprof.h

@@ -22,6 +22,12 @@ subject to the following restrictions:
 #ifndef QUICK_PROF_H
 #define QUICK_PROF_H
 
+#include "btScalar.h"
+
+//#define USE_QUICKPROF 1
+//Don't use quickprof for now, because it contains STL. TODO: replace STL by Bullet container classes.
+
+
 //if you don't need btClock, you can comment next line
 #define USE_BT_CLOCK 1
 
@@ -224,13 +230,10 @@ class btClock
 #endif //USE_BT_CLOCK
 
 
-//#define USE_QUICKPROF 1
-//Don't use quickprof for now, because it contains STL. TODO: replace STL by Bullet container classes.
-
 #ifdef USE_QUICKPROF
 
 
-//#include <iostream>
+#include <iostream>
 #include <fstream>
 #include <string>
 #include <map>
@@ -355,6 +358,7 @@ public:
 	/// Prints an error message to standard output.
 	inline static void printError(const std::string& msg)
 	{
+		//btAssert(0);
 		std::cout << "[QuickProf error] " << msg << std::endl;
 	}