improve handling of restitution by using the velocity (linear/angular) before applying forces: this is done by re-introducing the btSolverBody and only apply the forces to solver body, and use the original rigid body velocity for restitution computation.

warmstarting for contact points was broken, fix in btPersistentManifold
enable split impulse by default (at the cost of some performance)
add  the option for zero-length friction (instead of recomputing friction directions using btPlaneSpace), use the solver mode flag SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS
precompute lateral friction directions (in btManifoldResult)
remove the mConstraintRow[3] from btManifoldPoint, it just took a lot of memory with no benefits: fixed it in btParallelConstraintSolver

src/BulletCollision/CollisionDispatch/btManifoldResult.cpp

@@ -92,6 +92,10 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
 	newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
 	newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
 
+	btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2);
+	
+
+	
    //BP mod, store contact triangles.
 	if (isSwapped)
 	{

src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

@@ -76,9 +76,7 @@ class btManifoldPoint
 					m_contactCFM2(0.f),
 					m_lifeTime(0)
 			{
-				mConstraintRow[0].m_accumImpulse = 0.f;
+				
-				mConstraintRow[1].m_accumImpulse = 0.f;
-				mConstraintRow[2].m_accumImpulse = 0.f;
 			}
 
 			
@@ -101,9 +99,9 @@ class btManifoldPoint
 			int			m_index1;
 				
 			mutable void*	m_userPersistentData;
-			btScalar		m_appliedImpulse;
-
 			bool			m_lateralFrictionInitialized;
+
+			btScalar		m_appliedImpulse;
 			btScalar		m_appliedImpulseLateral1;
 			btScalar		m_appliedImpulseLateral2;
 			btScalar		m_contactMotion1;
@@ -118,8 +116,6 @@ class btManifoldPoint
 
 
 
-			btConstraintRow mConstraintRow[3];
-
 
 			btScalar getDistance() const
 			{

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

@@ -143,10 +143,6 @@ public:
 			m_pointCache[index] = m_pointCache[lastUsedIndex]; 
 			//get rid of duplicated userPersistentData pointer
 			m_pointCache[lastUsedIndex].m_userPersistentData = 0;
-			m_pointCache[lastUsedIndex].mConstraintRow[0].m_accumImpulse = 0.f;
-			m_pointCache[lastUsedIndex].mConstraintRow[1].m_accumImpulse = 0.f;
-			m_pointCache[lastUsedIndex].mConstraintRow[2].m_accumImpulse = 0.f;
-
 			m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f;
 			m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false;
 			m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
@@ -164,9 +160,9 @@ public:
 #define MAINTAIN_PERSISTENCY 1
 #ifdef MAINTAIN_PERSISTENCY
 		int	lifeTime = m_pointCache[insertIndex].getLifeTime();
-		btScalar	appliedImpulse = m_pointCache[insertIndex].mConstraintRow[0].m_accumImpulse;
+		btScalar	appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
-		btScalar	appliedLateralImpulse1 = m_pointCache[insertIndex].mConstraintRow[1].m_accumImpulse;
+		btScalar	appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
-		btScalar	appliedLateralImpulse2 = m_pointCache[insertIndex].mConstraintRow[2].m_accumImpulse;
+		btScalar	appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
 //		bool isLateralFrictionInitialized = m_pointCache[insertIndex].m_lateralFrictionInitialized;
 		
 		
@@ -181,9 +177,9 @@ public:
 		m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
 		m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
 		
-		m_pointCache[insertIndex].mConstraintRow[0].m_accumImpulse =  appliedImpulse;
+		m_pointCache[insertIndex].m_appliedImpulse =  appliedImpulse;
-		m_pointCache[insertIndex].mConstraintRow[1].m_accumImpulse = appliedLateralImpulse1;
+		m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
-		m_pointCache[insertIndex].mConstraintRow[2].m_accumImpulse = appliedLateralImpulse2;
+		m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
 
 
 		m_pointCache[insertIndex].m_lifeTime = lifeTime;

src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

@@ -304,7 +304,7 @@ void	btConeTwistConstraint::buildJacobian()
 
 
 
-void	btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar	timeStep)
+void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
 {
 	#ifndef __SPU__
 	if (m_useSolveConstraintObsolete)

src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

@@ -142,7 +142,7 @@ public:
 	
 	void	getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
 
-	virtual	void	solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar	timeStep);
+	virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
 
 	void	updateRHS(btScalar	timeStep);
 

src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -16,18 +16,20 @@ subject to the following restrictions:
 #ifndef BT_CONTACT_SOLVER_INFO
 #define BT_CONTACT_SOLVER_INFO
 
+#include "LinearMath/btScalar.h"
+
 enum	btSolverMode
 {
 	SOLVER_RANDMIZE_ORDER = 1,
 	SOLVER_FRICTION_SEPARATE = 2,
 	SOLVER_USE_WARMSTARTING = 4,
-	SOLVER_USE_FRICTION_WARMSTARTING = 8,
 	SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
 	SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
 	SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
 	SOLVER_CACHE_FRIENDLY = 128,
-	SOLVER_SIMD = 256,	//enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version
+	SOLVER_SIMD = 256,
-	SOLVER_CUDA = 512	//will be open sourced during Game Developers Conference 2009. Much faster.
+	SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
+	SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024
 };
 
 struct btContactSolverInfoData
@@ -67,6 +69,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_tau = btScalar(0.6);
 		m_damping = btScalar(1.0);
 		m_friction = btScalar(0.3);
+		m_timeStep = btScalar(1.f/60.f);
 		m_restitution = btScalar(0.);
 		m_maxErrorReduction = btScalar(20.);
 		m_numIterations = 10;
@@ -74,12 +77,12 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_erp2 = btScalar(0.1);
 		m_globalCfm = btScalar(0.);
 		m_sor = btScalar(1.);
-		m_splitImpulse = false;
+		m_splitImpulse = true;
-		m_splitImpulsePenetrationThreshold = -0.02f;
+		m_splitImpulsePenetrationThreshold = -.04f;
 		m_linearSlop = btScalar(0.0);
 		m_warmstartingFactor=btScalar(0.85);
+		//m_solverMode =  SOLVER_USE_WARMSTARTING |  SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER;
 		m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
-		m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
 		m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
 	}
 };

src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

@@ -174,10 +174,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
 
 	// current velocity difference
 
-	btVector3 angVelA;
+	btVector3 angVelA = body0->getAngularVelocity();
-	body0->internalGetAngularVelocity(angVelA);
+	btVector3 angVelB = body1->getAngularVelocity();
-	btVector3 angVelB;
-	body1->internalGetAngularVelocity(angVelB);
 
 	btVector3 vel_diff;
 	vel_diff = angVelA-angVelB;
@@ -225,12 +223,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
 
 	btVector3 motorImp = clippedMotorImpulse * axis;
 
-	//body0->applyTorqueImpulse(motorImp);
+	body0->applyTorqueImpulse(motorImp);
-	//body1->applyTorqueImpulse(-motorImp);
+	body1->applyTorqueImpulse(-motorImp);
-
-	body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
-	body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
-
 
 	return clippedMotorImpulse;
 
@@ -292,10 +286,8 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
 	btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
 	btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
 
-	btVector3 vel1;
+	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-	body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
+	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-	btVector3 vel2;
-	body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
 	btVector3 vel = vel1 - vel2;
 
 	btScalar rel_vel = axis_normal_on_a.dot(vel);
@@ -348,14 +340,8 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
 	normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
 
 	btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
-	//body1.applyImpulse( impulse_vector, rel_pos1);
+	body1.applyImpulse( impulse_vector, rel_pos1);
-	//body2.applyImpulse(-impulse_vector, rel_pos2);
+	body2.applyImpulse(-impulse_vector, rel_pos2);
-
-	btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
-	btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
-	body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
-	body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
-
 
 	
 

src/BulletDynamics/ConstraintSolver/btJacobianEntry.h

@@ -16,8 +16,7 @@ subject to the following restrictions:
 #ifndef BT_JACOBIAN_ENTRY_H
 #define BT_JACOBIAN_ENTRY_H
 
-#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
-#include "BulletDynamics/Dynamics/btRigidBody.h"
 
 
 //notes:

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -18,24 +18,23 @@ subject to the following restrictions:
 
 #include "btSequentialImpulseConstraintSolver.h"
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
-#include "BulletDynamics/Dynamics/btRigidBody.h"
+
-#include "btContactConstraint.h"
-#include "btSolve2LinearConstraint.h"
-#include "btContactSolverInfo.h"
 #include "LinearMath/btIDebugDraw.h"
-#include "btJacobianEntry.h"
+//#include "btJacobianEntry.h"
 #include "LinearMath/btMinMax.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
 #include <new>
 #include "LinearMath/btStackAlloc.h"
 #include "LinearMath/btQuickprof.h"
-#include "btSolverBody.h"
+//#include "btSolverBody.h"
-#include "btSolverConstraint.h"
+//#include "btSolverConstraint.h"
 #include "LinearMath/btAlignedObjectArray.h"
 #include <string.h> //for memset
 
 int		gNumSplitImpulseRecoveries = 0;
 
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+
 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
 :m_btSeed2(0)
 {
@@ -57,7 +56,7 @@ static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
 #endif//USE_SIMD
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
-void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
+void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 #ifdef USE_SIMD
 	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
@@ -91,7 +90,7 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 }
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
 	const btScalar deltaVel1Dotn	=	c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
@@ -120,7 +119,7 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 		body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
 }
 
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 #ifdef USE_SIMD
 	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
@@ -151,7 +150,7 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 }
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
 	const btScalar deltaVel1Dotn	=	c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
@@ -175,8 +174,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 
 
 void	btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
-        btRigidBody& body1,
+        btSolverBody& body1,
-        btRigidBody& body2,
+        btSolverBody& body2,
         const btSolverConstraint& c)
 {
 		if (c.m_rhsPenetration)
@@ -203,7 +202,7 @@ void	btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
         }
 }
 
- void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
+ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 #ifdef USE_SIMD
 	if (!c.m_rhsPenetration)
@@ -277,9 +276,10 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
 }
 
 
-#if 0
+
 void	btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
 {
+
 	btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
 
 	solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
@@ -289,17 +289,27 @@ void	btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod
 
 	if (rb)
 	{
-		solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor();
+		solverBody->m_worldTransform = rb->getWorldTransform();
+		solverBody->internalSetInvMass(btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor());
 		solverBody->m_originalBody = rb;
 		solverBody->m_angularFactor = rb->getAngularFactor();
+		solverBody->m_linearFactor = rb->getLinearFactor();
+		solverBody->m_linearVelocity = rb->getLinearVelocity();
+		solverBody->m_angularVelocity = rb->getAngularVelocity();
 	} else
 	{
-		solverBody->internalGetInvMass().setValue(0,0,0);
+		solverBody->m_worldTransform.setIdentity();
+		solverBody->internalSetInvMass(btVector3(0,0,0));
 		solverBody->m_originalBody = 0;
 		solverBody->m_angularFactor.setValue(1,1,1);
+		solverBody->m_linearFactor.setValue(1,1,1);
+		solverBody->m_linearVelocity.setValue(0,0,0);
+		solverBody->m_angularVelocity.setValue(0,0,0);
 	}
+
+
 }
-#endif
+
 
 
 
@@ -313,9 +323,11 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel,
 
 
 
-void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
+static void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
-void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
+static void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
 {
+	
+
 	if (colObj && colObj->hasAnisotropicFriction())
 	{
 		// transform to local coordinates
@@ -326,20 +338,23 @@ void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirec
 		// ... and transform it back to global coordinates
 		frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
 	}
+
 }
 
 
-void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
+void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int  solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
 {
 
 	
-	btRigidBody* body0=btRigidBody::upcast(colObj0);
-	btRigidBody* body1=btRigidBody::upcast(colObj1);
-
 	solverConstraint.m_contactNormal = normalAxis;
+	btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
+	btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
 
-	solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody();
+	btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
-	solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody();
+	btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
+
+	solverConstraint.m_solverBodyIdA = solverBodyIdA;
+	solverConstraint.m_solverBodyIdB = solverBodyIdB;
 
 	solverConstraint.m_friction = cp.m_combinedFriction;
 	solverConstraint.m_originalContactPoint = 0;
@@ -392,11 +407,13 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 
 
 	{
+		
+
 		btScalar rel_vel;
-		btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0)) 
+		btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0)) 
-			+ solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
+			+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
-		btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0)) 
+		btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0)) 
-			+ solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
+			+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
 
 		rel_vel = vel1Dotn+vel2Dotn;
 
@@ -408,23 +425,24 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 		solverConstraint.m_cfm = cfmSlip;
 		solverConstraint.m_lowerLimit = 0;
 		solverConstraint.m_upperLimit = 1e10f;
+		
 	}
 }
 
 
 
-btSolverConstraint&	btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
+btSolverConstraint&	btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
 {
 	btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
 	solverConstraint.m_frictionIndex = frictionIndex;
-	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2, 
+	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, 
 							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
 
 int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
 {
-#if 0
+
 	int solverBodyIdA = -1;
 
 	if (body.getCompanionId() >= 0)
@@ -445,29 +463,33 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 			return 0;//assume first one is a fixed solver body
 		}
 	}
+
 	return solverBodyIdA;
-#endif
+
-	return 0;
 }
 #include <stdio.h>
 
 
 void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, 
-																 btCollisionObject* colObj0, btCollisionObject* colObj1,
+																 int solverBodyIdA, int solverBodyIdB,
 																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
 																 btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
 																 btVector3& rel_pos1, btVector3& rel_pos2)
 {
-			btRigidBody* rb0 = btRigidBody::upcast(colObj0);
-			btRigidBody* rb1 = btRigidBody::upcast(colObj1);
 			
 			const btVector3& pos1 = cp.getPositionWorldOnA();
 			const btVector3& pos2 = cp.getPositionWorldOnB();
 
+			btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
+			btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
+
+			btRigidBody* rb0 = bodyA->m_originalBody;
+			btRigidBody* rb1 = bodyB->m_originalBody;
+
 //			btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
 //			btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
-			rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
+			rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
-			rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
+			rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
 
 			relaxation = 1.f;
 
@@ -501,29 +523,27 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 				}
 
 				solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
-				solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
+				solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-				solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
+				solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
 
+				btScalar restitution = 0.f;
+				btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
 
+				{
+					btVector3 vel1,vel2;
 
+					vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
+					vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
 
-			btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
+	//			btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
-			btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
 					vel  = vel1 - vel2;
 					rel_vel = cp.m_normalWorldOnB.dot(vel);
 
-				btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
 					
 
 					solverConstraint.m_friction = cp.m_combinedFriction;
 
-				btScalar restitution = 0.f;
 				
-				if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
-				{
-					restitution = 0.f;
-				} else
-				{
 					restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
 					if (restitution <= btScalar(0.))
 					{
@@ -537,9 +557,9 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 				{
 					solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
 					if (rb0)
-						rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+						bodyA->internalApplyImpulse(solverConstraint.m_contactNormal*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
 					if (rb1)
-						rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
+						bodyB->internalApplyImpulse(solverConstraint.m_contactNormal*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
 				} else
 				{
 					solverConstraint.m_appliedImpulse = 0.f;
@@ -548,33 +568,41 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 				solverConstraint.m_appliedPushImpulse = 0.f;
 
 				{
-					btScalar rel_vel;
+					btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?bodyA->m_linearVelocity:btVector3(0,0,0)) 
-					btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) 
+						+ solverConstraint.m_relpos1CrossNormal.dot(rb0?bodyA->m_angularVelocity:btVector3(0,0,0));
-						+ solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
+					btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?bodyB->m_linearVelocity:btVector3(0,0,0)) 
-					btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0)) 
+						+ solverConstraint.m_relpos2CrossNormal.dot(rb1?bodyB->m_angularVelocity:btVector3(0,0,0));
-						+ solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
+					btScalar rel_vel = vel1Dotn+vel2Dotn;
-
-					rel_vel = vel1Dotn+vel2Dotn;
 
 					btScalar positionalError = 0.f;
 					btScalar	velocityError = restitution - rel_vel;// * damping;
 					
+
+					btScalar 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 * infoGlobal.m_erp/infoGlobal.m_timeStep;
+						positionalError = -penetration * erp/infoGlobal.m_timeStep;
 					}
 
 					btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
 					btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+
 					if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 					{
 						//combine position and velocity into rhs
 						solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
 						solverConstraint.m_rhsPenetration = 0.f;
+
 					} else
 					{
 						//split position and velocity into rhs and m_rhsPenetration
@@ -594,20 +622,25 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 
 
 void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, 
-																		btRigidBody* rb0, btRigidBody* rb1, 
+																		int solverBodyIdA, int solverBodyIdB,
 																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
 {
-					if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
+
-					{
+	btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
+	btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
+
+	btRigidBody* rb0 = bodyA->m_originalBody;
+	btRigidBody* rb1 = bodyB->m_originalBody;
+
 	{
 		btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
 		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 		{
 			frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
 			if (rb0)
-									rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
+				bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
 			if (rb1)
-									rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
+				bodyB->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
 		} else
 		{
 			frictionConstraint1.m_appliedImpulse = 0.f;
@@ -621,24 +654,14 @@ void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolver
 		{
 			frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2  * infoGlobal.m_warmstartingFactor;
 			if (rb0)
-									rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
+				bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
 			if (rb1)
-									rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
+				bodyB->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
 		} else
 		{
 			frictionConstraint2.m_appliedImpulse = 0.f;
 		}
 	}
-					} else
-					{
-						btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
-						frictionConstraint1.m_appliedImpulse = 0.f;
-						if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-						{
-							btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
-							frictionConstraint2.m_appliedImpulse = 0.f;
-						}
-					}
 }
 
 
@@ -651,12 +674,19 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 	colObj0 = (btCollisionObject*)manifold->getBody0();
 	colObj1 = (btCollisionObject*)manifold->getBody1();
 
+	int solverBodyIdA = getOrInitSolverBody(*colObj0);
+	int solverBodyIdB = getOrInitSolverBody(*colObj1);
+
+	btRigidBody* bodyA = btRigidBody::upcast(colObj0);
+	btRigidBody* bodyB = btRigidBody::upcast(colObj1);
+
+	btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
+	btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
+
 
-	btRigidBody* solverBodyA = btRigidBody::upcast(colObj0);
-	btRigidBody* solverBodyB = btRigidBody::upcast(colObj1);
 
 	///avoid collision response between two static objects
-	if ((!solverBodyA || !solverBodyA->getInvMass()) && (!solverBodyB || !solverBodyB->getInvMass()))
+	if (!solverBodyA || !solverBodyA->m_originalBody && (!solverBodyB || !solverBodyB->m_originalBody))
 		return;
 
 	for (int j=0;j<manifold->getNumContacts();j++)
@@ -676,11 +706,12 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
 			btRigidBody* rb0 = btRigidBody::upcast(colObj0);
 			btRigidBody* rb1 = btRigidBody::upcast(colObj1);
-			solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody();
+			solverConstraint.m_solverBodyIdA = solverBodyIdA;
-			solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody();
+			solverConstraint.m_solverBodyIdB = solverBodyIdB;
+
 			solverConstraint.m_originalContactPoint = &cp;
 
-			setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
+			setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
 
 //			const btVector3& pos1 = cp.getPositionWorldOnA();
 //			const btVector3& pos2 = cp.getPositionWorldOnB();
@@ -695,46 +726,47 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 				btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
 				if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
 				{
-					cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
+					cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
 					if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
 						cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
 						cp.m_lateralFrictionDir2.normalize();//??
 						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
 						applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
-						addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+						addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 					}
 
 					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
 					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
-					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 					cp.m_lateralFrictionInitialized = true;
 				} else
 				{
-					//re-calculate friction direction every frame, todo: check if this is really needed
+					if (!(infoGlobal.m_solverMode & SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS))
 						btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
 						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
 						applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
-						addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+						addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 					}
 
 					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
 					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
-					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 
 					cp.m_lateralFrictionInitialized = true;
 				}
 
 			} else
 			{
-				addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
+				addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
 				if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-					addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
+					addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
 			}
 			
-			setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal);
+			setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
 
 		}
 	}
@@ -748,36 +780,31 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 	m_maxOverrideNumSolverIterations = 0;
 
-	if (!(numConstraints + numManifolds))
+	
+	
+	for (int i = 0; i < numBodies; i++)
 	{
-		//		printf("empty\n");
+		bodies[i]->setCompanionId(-1);
-		return 0.f;
 	}
 
-	if (infoGlobal.m_splitImpulse)
+
-	{
+	m_tmpSolverBodyPool.reserve(numBodies+1);
-		for (int i = 0; i < numBodies; i++)
+	m_tmpSolverBodyPool.resize(0);
+
+	btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+    initSolverBody(&fixedBody,0);
+
+	//convert all bodies
+
+	for (int i=0;i<numBodies;i++)
 	{
+		int bodyId = getOrInitSolverBody(*bodies[i]);
 		btRigidBody* body = btRigidBody::upcast(bodies[i]);
-			if (body)
+		if (body && body->getInvMass())
 		{
-				body->internalGetDeltaLinearVelocity().setZero();
+			btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
-				body->internalGetDeltaAngularVelocity().setZero();
+			solverBody.m_linearVelocity += body->getTotalForce()*body->getInvMass()*infoGlobal.m_timeStep;
-				body->internalGetPushVelocity().setZero();
+			solverBody.m_angularVelocity += body->getTotalTorque()*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
-				body->internalGetTurnVelocity().setZero();
-			}
-		}
-	}
-	else
-	{
-		for (int i = 0; i < numBodies; i++)
-		{
-			btRigidBody* body = btRigidBody::upcast(bodies[i]);
-			if (body)
-			{	
-				body->internalGetDeltaLinearVelocity().setZero();
-				body->internalGetDeltaAngularVelocity().setZero();
-			}
 		}
 	}
 	
@@ -791,6 +818,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 			constraint->internalSetAppliedImpulse(0.0f);
 		}
 	}
+
 	//btRigidBody* rb0=0,*rb1=0;
 
 	//if (1)
@@ -834,6 +862,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 					btRigidBody& rbA = constraint->getRigidBodyA();
 					btRigidBody& rbB = constraint->getRigidBodyB();
 
+                    int solverBodyIdA = getOrInitSolverBody(rbA);
+                    int solverBodyIdB = getOrInitSolverBody(rbB);
+
+                    btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
+                    btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
+
+
+
 
 					int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
 					if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
@@ -848,16 +884,19 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 						currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
 						currentConstraintRow[j].m_appliedImpulse = 0.f;
 						currentConstraintRow[j].m_appliedPushImpulse = 0.f;
-						currentConstraintRow[j].m_solverBodyA = &rbA;
+						currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
-						currentConstraintRow[j].m_solverBodyB = &rbB;
+						currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
 						currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
 					}
 
-					rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
+					bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
-					rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
+					bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
-					rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
+					bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
-					rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
+					bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
-
+					bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
+					bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
+					bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
+					bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
 
 
 					btTypedConstraint::btConstraintInfo2 info2;
@@ -967,7 +1006,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 	///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
 	m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
+	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+		m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2);
+	else
 		m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
+
 	m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
 	{
 		int i;
@@ -996,12 +1039,12 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
 	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
 
-	int j;
 	
 	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
 	{
 		if ((iteration & 7) == 0) {
-			for (j=0; j<numNonContactPool; ++j) {
+
+			for (int j=0; j<numNonContactPool; ++j) {
 				int tmp = m_orderNonContactConstraintPool[j];
 				int swapi = btRandInt2(j+1);
 				m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
@@ -1011,14 +1054,14 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 			//contact/friction constraints are not solved more than 
 			if (iteration< infoGlobal.m_numIterations)
 			{
-				for (j=0; j<numConstraintPool; ++j) {
+				for (int j=0; j<numConstraintPool; ++j) {
 					int tmp = m_orderTmpConstraintPool[j];
 					int swapi = btRandInt2(j+1);
 					m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
 					m_orderTmpConstraintPool[swapi] = tmp;
 				}
 
-				for (j=0; j<numFrictionPool; ++j) {
+				for (int j=0; j<numFrictionPool; ++j) {
 					int tmp = m_orderFrictionConstraintPool[j];
 					int swapi = btRandInt2(j+1);
 					m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
@@ -1031,26 +1074,78 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 	if (infoGlobal.m_solverMode & SOLVER_SIMD)
 	{
 		///solve all joint constraints, using SIMD, if available
-		for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
 		{
 			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
 			if (iteration < constraint.m_overrideNumSolverIterations)
-				resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
+				resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
 		}
 
 		if (iteration< infoGlobal.m_numIterations)
 		{
-			for (j=0;j<numConstraints;j++)
+			for (int j=0;j<numConstraints;j++)
 			{
 				constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
 			}
 
 			///solve all contact constraints using SIMD, if available
+			if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
+			{
 				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+				int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
+
+				for (int c=0;c<numPoolConstraints;c++)
+				{
+					btScalar totalImpulse =0;
+
+					{
+						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
+						resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						totalImpulse = solveManifold.m_appliedImpulse;
+					}
+					bool applyFriction = true;
+					if (applyFriction)
+					{
+						{
+
+							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
+
+							if (totalImpulse>btScalar(0))
+							{
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+								resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+							}
+						}
+
+						if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
+						{
+
+							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
+				
+							if (totalImpulse>btScalar(0))
+							{
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+								resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+							}
+						}
+					}
+				}
+
+			}
+			else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
+			{
+				//solve the friction constraints after all contact constraints, don't interleave them
+				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+				int j;
+
 				for (j=0;j<numPoolConstraints;j++)
 				{
 					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-				resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+					resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 
 				}
 		
@@ -1066,37 +1161,38 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
 						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
 
-					resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA,	*solveManifold.m_solverBodyB,solveManifold);
+						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					}
 				}
 			}			
 		}
 	} else
 	{
-
+		//non-SIMD version
 		///solve all joint constraints
-		for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
 		{
 			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
 			if (iteration < constraint.m_overrideNumSolverIterations)
-				resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
+				resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
 		}
 
 		if (iteration< infoGlobal.m_numIterations)
 		{
-			for (j=0;j<numConstraints;j++)
+			for (int j=0;j<numConstraints;j++)
 			{
 				constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
 			}
 			///solve all contact constraints
 			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-			for (j=0;j<numPoolConstraints;j++)
+			for (int j=0;j<numPoolConstraints;j++)
 			{
 				const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-				resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+				resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 			}
 			///solve all friction constraints
 			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-			for (j=0;j<numFrictionPoolConstraints;j++)
+			for (int j=0;j<numFrictionPoolConstraints;j++)
 			{
 				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
 				btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
@@ -1106,7 +1202,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 					solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
 					solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
 
-					resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+					resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 				}
 			}
 		}
@@ -1131,7 +1227,7 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
 					{
 						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
 
-						resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+						resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 					}
 				}
 			}
@@ -1147,7 +1243,7 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
 					{
 						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
 
-						resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+						resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 					}
 				}
 			}
@@ -1175,26 +1271,30 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
 	return 0.f;
 }
 
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** /*constraints*/,int /* numConstraints*/,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
 {
 	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
 	int i,j;
 
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	{
 		for (j=0;j<numPoolConstraints;j++)
 		{
-
 			const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
 			btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
 			btAssert(pt);
 			pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
-		if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
+		//	float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-		{
+			//	printf("pt->m_appliedImpulseLateral1 = %f\n", f);
 			pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+			//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
+			if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+			{
 				pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
 			}
-
 			//do a callback here?
 		}
+	}
 
 	numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
 	for (j=0;j<numPoolConstraints;j++)
@@ -1209,29 +1309,41 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
 	}
 
 
+
 	if (infoGlobal.m_splitImpulse)
 	{		
-		for ( i=0;i<numBodies;i++)
+		for ( i=0;i<m_tmpSolverBodyPool.size();i++)
 		{
-			btRigidBody* body = btRigidBody::upcast(bodies[i]);
+			btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
 			if (body)
-				body->internalWritebackVelocity(infoGlobal.m_timeStep);
+			{
+				m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep);
+				m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity);
+				m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity);
+				m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
+
+			}
 		}
 	} else
 	{
-		for ( i=0;i<numBodies;i++)
+		for ( i=0;i<m_tmpSolverBodyPool.size();i++)
 		{
-			btRigidBody* body = btRigidBody::upcast(bodies[i]);
+			btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
 			if (body)
-				body->internalWritebackVelocity();
+			{
+				m_tmpSolverBodyPool[i].writebackVelocity();
+				m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity);
+				m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity);
 			}
 		}
+	}
+
 	
 
 	m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
 	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
 	m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
-
+	m_tmpSolverBodyPool.resizeNoInitialize(0);
 	return 0.f;
 }
 
@@ -1243,14 +1355,12 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
 
 	BT_PROFILE("solveGroup");
 	//you need to provide at least some bodies
-	btAssert(bodies);
-	btAssert(numBodies);
 	
 	solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
 
 	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
 
-	solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+	solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
 	
 	return 0.f;
 }
@@ -1260,10 +1370,4 @@ void	btSequentialImpulseConstraintSolver::reset()
 	m_btSeed2 = 0;
 }
 
-btRigidBody& btSequentialImpulseConstraintSolver::getFixedBody()
-{
-	static btRigidBody s_fixed(0, 0,0);
-	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
-	return s_fixed;
-}
 

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -16,19 +16,23 @@ subject to the following restrictions:
 #ifndef BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
 #define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
 
-#include "btConstraintSolver.h"
 class btIDebugDraw;
-#include "btContactConstraint.h"
+class btPersistentManifold;
-#include "btSolverBody.h"
+class btStackAlloc;
-#include "btSolverConstraint.h"
+class btDispatcher;
-#include "btTypedConstraint.h"
+class btCollisionObject;
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
+#include "BulletDynamics/ConstraintSolver/btSolverConstraint.h"
 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
+#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
 
 ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
 ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
 protected:
-
+	btAlignedObjectArray<btSolverBody>      m_tmpSolverBodyPool;
 	btConstraintArray			m_tmpSolverContactConstraintPool;
 	btConstraintArray			m_tmpSolverNonContactConstraintPool;
 	btConstraintArray			m_tmpSolverContactFrictionConstraintPool;
@@ -38,55 +42,54 @@ protected:
 	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
 	int							m_maxOverrideNumSolverIterations;
 
-	void setupFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyIdB,
+	void setupFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
 									btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
 									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
 									btScalar desiredVelocity=0., btScalar cfmSlip=0.);
 
-	btSolverConstraint&	addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
+	btSolverConstraint&	addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
 	
-	void setupContactConstraint(btSolverConstraint& solverConstraint, btCollisionObject* colObj0, btCollisionObject* colObj1, btManifoldPoint& cp, 
+	void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, 
 								const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation, 
 								btVector3& rel_pos1, btVector3& rel_pos2);
 
-	void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, btRigidBody* rb0, btRigidBody* rb1, 
+	void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, 
 										 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
 
 	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
 	unsigned long	m_btSeed2;
 
-//	void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
+	
 	btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
 
 	void	convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
 
 
 	void	resolveSplitPenetrationSIMD(
-        btRigidBody& body1,
+     btSolverBody& bodyA,btSolverBody& bodyB,
-        btRigidBody& body2,
         const btSolverConstraint& contactConstraint);
 
 	void	resolveSplitPenetrationImpulseCacheFriendly(
-        btRigidBody& body1,
+       btSolverBody& bodyA,btSolverBody& bodyB,
-        btRigidBody& body2,
         const btSolverConstraint& contactConstraint);
 
 	//internal method
 	int		getOrInitSolverBody(btCollisionObject& body);
+	void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
 
-	void	resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
+	void	resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
 
-	void	resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
+	void	resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
 	
-	void	resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
+	void	resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
 	
-	void	resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
+	void	resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
 		
 protected:
-	static btRigidBody& getFixedBody();
+	
 	
 	virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
-	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
 	btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
 
 	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
@@ -122,9 +125,7 @@ public:
 
 };
 
-#ifndef BT_PREFER_SIMD
+
-typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverPrefered;
-#endif
 
 
 #endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H

src/BulletDynamics/ConstraintSolver/btSolverBody.h

@@ -19,7 +19,7 @@ subject to the following restrictions:
 class	btRigidBody;
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btMatrix3x3.h"
-#include "BulletDynamics/Dynamics/btRigidBody.h"
+
 #include "LinearMath/btAlignedAllocator.h"
 #include "LinearMath/btTransformUtil.h"
 
@@ -105,22 +105,35 @@ operator+(const btSimdScalar& v1, const btSimdScalar& v2)
 #endif
 
 ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
-ATTRIBUTE_ALIGNED64 (struct)	btSolverBodyObsolete
+ATTRIBUTE_ALIGNED64 (struct)	btSolverBody
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
+	btTransform		m_worldTransform;
 	btVector3		m_deltaLinearVelocity;
 	btVector3		m_deltaAngularVelocity;
 	btVector3		m_angularFactor;
+	btVector3		m_linearFactor;
 	btVector3		m_invMass;
-	btRigidBody*	m_originalBody;
 	btVector3		m_pushVelocity;
 	btVector3		m_turnVelocity;
+	btVector3		m_linearVelocity;
+	btVector3		m_angularVelocity;
 
+	btRigidBody*	m_originalBody;
+	void	setWorldTransform(const btTransform& worldTransform)
+	{
+		m_worldTransform = worldTransform;
+	}
+
+	const btTransform& getWorldTransform() const
+	{
+		return m_worldTransform;
+	}
 	
 	SIMD_FORCE_INLINE void	getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
 	{
 		if (m_originalBody)
-			velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
+			velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
 		else
 			velocity.setValue(0,0,0);
 	}
@@ -128,7 +141,7 @@ ATTRIBUTE_ALIGNED64 (struct)	btSolverBodyObsolete
 	SIMD_FORCE_INLINE void	getAngularVelocity(btVector3& angVel) const
 	{
 		if (m_originalBody)
-			angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
+			angVel =m_angularVelocity+m_deltaAngularVelocity;
 		else
 			angVel.setValue(0,0,0);
 	}
@@ -137,9 +150,9 @@ ATTRIBUTE_ALIGNED64 (struct)	btSolverBodyObsolete
 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
 	SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
 	{
-		//if (m_invMass)
+		if (m_originalBody)
 		{
-			m_deltaLinearVelocity += linearComponent*impulseMagnitude;
+			m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
 			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
 		}
 	}
@@ -148,36 +161,125 @@ ATTRIBUTE_ALIGNED64 (struct)	btSolverBodyObsolete
 	{
 		if (m_originalBody)
 		{
-			m_pushVelocity += linearComponent*impulseMagnitude;
+			m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor;
 			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
 		}
 	}
 
+
+
+	const btVector3& getDeltaLinearVelocity() const
+	{
+		return m_deltaLinearVelocity;
+	}
+
+	const btVector3& getDeltaAngularVelocity() const
+	{
+		return m_deltaAngularVelocity;
+	}
+
+	const btVector3& getPushVelocity() const 
+	{
+		return m_pushVelocity;
+	}
+
+	const btVector3& getTurnVelocity() const 
+	{
+		return m_turnVelocity;
+	}
+
+
+	////////////////////////////////////////////////
+	///some internal methods, don't use them
+		
+	btVector3& internalGetDeltaLinearVelocity()
+	{
+		return m_deltaLinearVelocity;
+	}
+
+	btVector3& internalGetDeltaAngularVelocity()
+	{
+		return m_deltaAngularVelocity;
+	}
+
+	const btVector3& internalGetAngularFactor() const
+	{
+		return m_angularFactor;
+	}
+
+	const btVector3& internalGetInvMass() const
+	{
+		return m_invMass;
+	}
+
+	void internalSetInvMass(const btVector3& invMass)
+	{
+		m_invMass = invMass;
+	}
+	
+	btVector3& internalGetPushVelocity()
+	{
+		return m_pushVelocity;
+	}
+
+	btVector3& internalGetTurnVelocity()
+	{
+		return m_turnVelocity;
+	}
+
+	SIMD_FORCE_INLINE void	internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
+	{
+		velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
+	}
+
+	SIMD_FORCE_INLINE void	internalGetAngularVelocity(btVector3& angVel) const
+	{
+		angVel = m_angularVelocity+m_deltaAngularVelocity;
+	}
+
+
+	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
+	{
+		if (m_originalBody)
+		{
+			m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
+			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		}
+	}
+		
+	
+	
+
 	void	writebackVelocity()
 	{
 		if (m_originalBody)
 		{
-			m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
+			m_linearVelocity +=m_deltaLinearVelocity;
-			m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
+			m_angularVelocity += m_deltaAngularVelocity;
 			
 			//m_originalBody->setCompanionId(-1);
 		}
 	}
 
 
-	void	writebackVelocity(btScalar timeStep)
+	void	writebackVelocityAndTransform(btScalar timeStep)
 	{
         (void) timeStep;
 		if (m_originalBody)
 		{
-			m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
+			m_linearVelocity += m_deltaLinearVelocity;
-			m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
+			m_angularVelocity += m_deltaAngularVelocity;
 			
 			//correct the position/orientation based on push/turn recovery
 			btTransform newTransform;
-			btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
+			if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0)
-			m_originalBody->setWorldTransform(newTransform);
+			{
-			
+				btQuaternion orn = m_worldTransform.getRotation();
+				btTransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity,timeStep,newTransform);
+				m_worldTransform = newTransform;
+			}
+			//m_worldTransform.setRotation(orn);
 			//m_originalBody->setCompanionId(-1);
 		}
 	}

src/BulletDynamics/ConstraintSolver/btSolverConstraint.h

@@ -20,6 +20,7 @@ class	btRigidBody;
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btMatrix3x3.h"
 #include "btJacobianEntry.h"
+#include "LinearMath/btAlignedObjectArray.h"
 
 //#define NO_FRICTION_TANGENTIALS 1
 #include "btSolverBody.h"
@@ -58,16 +59,10 @@ ATTRIBUTE_ALIGNED64 (struct)	btSolverConstraint
 		int			m_frictionIndex;
 		btScalar	m_unusedPadding1;
 	};
-	union
+	
-	{
+	int m_solverBodyIdA;
-		btRigidBody*	m_solverBodyA;
+	
-		int				m_companionIdA;
+	int m_solverBodyIdB;
-	};
-	union
-	{
-		btRigidBody*	m_solverBodyB;
-		int				m_companionIdB;
-	};
 	
 	union
 	{

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -133,12 +133,9 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 	virtual	void	processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
 	{
 		if (islandId<0)
-		{
-			if (numManifolds + m_numConstraints)
 		{
 			///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
 			m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
-			}
 		} else
 		{
 				//also add all non-contact constraints/joints for this island
@@ -165,12 +162,8 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 			}
 
 			if (m_solverInfo->m_minimumSolverBatchSize<=1)
-			{
-				///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
-				if (numManifolds + numCurConstraints)
 			{
 				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
-				}
 			} else
 			{
 				
@@ -191,8 +184,6 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 		}
 	}
 	void	processConstraints()
-	{
-		if (m_manifolds.size() + m_constraints.size()>0)
 	{
 
 		btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
@@ -200,7 +191,6 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
 		btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
 			
 		m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
-		}
 		m_bodies.resize(0);
 		m_manifolds.resize(0);
 		m_constraints.resize(0);
@@ -977,7 +967,8 @@ void	btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 		btRigidBody* body = m_nonStaticRigidBodies[i];
 		if (!body->isStaticOrKinematicObject())
 		{
-			body->integrateVelocities( timeStep);
+			//don't integrate/update velocities here, it happens in the constraint solver
+
 			//damping
 			body->applyDamping(timeStep);
 

src/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -250,7 +250,6 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
 }
 
 	
-
 void btRigidBody::updateInertiaTensor() 
 {
 	m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
@@ -317,26 +316,6 @@ bool btRigidBody::checkCollideWithOverride(const  btCollisionObject* co) const
 	return true;
 }
 
-void	btRigidBody::internalWritebackVelocity(btScalar timeStep)
-{
-    (void) timeStep;
-	if (m_inverseMass)
-	{
-		setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
-		setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
-		
-		//correct the position/orientation based on push/turn recovery
-		btTransform newTransform;
-		btTransformUtil::integrateTransform(getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
-		setWorldTransform(newTransform);
-		//m_originalBody->setCompanionId(-1);
-	}
-//	m_deltaLinearVelocity.setZero();
-//	m_deltaAngularVelocity .setZero();
-//	m_pushVelocity.setZero();
-//	m_turnVelocity.setZero();
-}
-
 
 
 void btRigidBody::addConstraintRef(btTypedConstraint* c)

src/BulletDynamics/Dynamics/btRigidBody.h

@@ -519,104 +519,6 @@ public:
 		return m_rigidbodyFlags;
 	}
 
-	const btVector3& getDeltaLinearVelocity() const
-	{
-		return m_deltaLinearVelocity;
-	}
-
-	const btVector3& getDeltaAngularVelocity() const
-	{
-		return m_deltaAngularVelocity;
-	}
-
-	const btVector3& getPushVelocity() const 
-	{
-		return m_pushVelocity;
-	}
-
-	const btVector3& getTurnVelocity() const 
-	{
-		return m_turnVelocity;
-	}
-
-
-	////////////////////////////////////////////////
-	///some internal methods, don't use them
-		
-	btVector3& internalGetDeltaLinearVelocity()
-	{
-		return m_deltaLinearVelocity;
-	}
-
-	btVector3& internalGetDeltaAngularVelocity()
-	{
-		return m_deltaAngularVelocity;
-	}
-
-	const btVector3& internalGetAngularFactor() const
-	{
-		return m_angularFactor;
-	}
-
-	const btVector3& internalGetInvMass() const
-	{
-		return m_invMass;
-	}
-	
-	btVector3& internalGetPushVelocity()
-	{
-		return m_pushVelocity;
-	}
-
-	btVector3& internalGetTurnVelocity()
-	{
-		return m_turnVelocity;
-	}
-
-	SIMD_FORCE_INLINE void	internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
-	{
-		velocity = getLinearVelocity()+m_deltaLinearVelocity + (getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
-	}
-
-	SIMD_FORCE_INLINE void	internalGetAngularVelocity(btVector3& angVel) const
-	{
-		angVel = getAngularVelocity()+m_deltaAngularVelocity;
-	}
-
-
-	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
-	{
-		if (m_inverseMass)
-		{
-			m_deltaLinearVelocity += linearComponent*impulseMagnitude;
-			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
-		}
-	}
-
-	SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
-	{
-		if (m_inverseMass)
-		{
-			m_pushVelocity += linearComponent*impulseMagnitude;
-			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
-		}
-	}
-	
-	void	internalWritebackVelocity()
-	{
-		if (m_inverseMass)
-		{
-			setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
-			setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
-			//m_deltaLinearVelocity.setZero();
-			//m_deltaAngularVelocity .setZero();
-			//m_originalBody->setCompanionId(-1);
-		}
-	}
-
-
-	void	internalWritebackVelocity(btScalar timeStep);
 	
 	
 

src/BulletMultiThreaded/btParallelConstraintSolver.cpp

@@ -19,7 +19,7 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
 #include "LinearMath/btPoolAllocator.h"
-
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 #include "BulletMultiThreaded/vectormath2bullet.h"
 
 #include "LinearMath/btQuickprof.h"
@@ -56,7 +56,6 @@ unsigned char ATTRIBUTE_ALIGNED128(tmp_buff[TMP_BUFF_BYTES]);
 	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
 	const btScalar deltaVel1Dotn	=	c.m_contactNormal.dot(getBtVector3(body1.mDeltaLinearVelocity)) 	+ c.m_relpos1CrossNormal.dot(getBtVector3(body1.mDeltaAngularVelocity));
 	const btScalar deltaVel2Dotn	=	-c.m_contactNormal.dot(getBtVector3(body2.mDeltaLinearVelocity)) + c.m_relpos2CrossNormal.dot(getBtVector3(body2.mDeltaAngularVelocity));
-
 //	const btScalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;
 	deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
 	deltaImpulse	-=	deltaVel2Dotn*c.m_jacDiagABInv;
@@ -160,6 +159,7 @@ void CustomSolveConstraintsTaskParallel(
 	const PfxParallelGroup *contactParallelGroup,const PfxParallelBatch *contactParallelBatches,
 	PfxConstraintPair *contactPairs,uint32_t numContactPairs,
 	btPersistentManifold* offsetContactManifolds,
+	btConstraintRow*	offsetContactConstraintRows,
 	const PfxParallelGroup *jointParallelGroup,const PfxParallelBatch *jointParallelBatches,
 	PfxConstraintPair *jointPairs,uint32_t numJointPairs,
 	btSolverConstraint* offsetSolverConstraints,
@@ -222,8 +222,9 @@ void CustomSolveConstraintsTaskParallel(
 						uint16_t iA = pfxGetRigidBodyIdA(pair);
 						uint16_t iB = pfxGetRigidBodyIdB(pair);
 
-						btPersistentManifold& contact = offsetContactManifolds[pfxGetConstraintId1(pair)];
+						uint32_t contactIndex = pfxGetConstraintId1(pair);
-
+						btPersistentManifold& contact = offsetContactManifolds[contactIndex];
+						btConstraintRow* contactConstraintRows = &offsetContactConstraintRows[contactIndex*12];
 
 						PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
 						PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
@@ -235,9 +236,17 @@ void CustomSolveConstraintsTaskParallel(
 								vmVector3 rA = rotate(solverBodyA.mOrientation,btReadVector3(cp.m_localPointA));
 								vmVector3 rB = rotate(solverBodyB.mOrientation,btReadVector3(cp.m_localPointB));
 								
-								for(int k=0;k<3;k++) {
+								float imp[3] = 
-									vmVector3 normal = btReadVector3(cp.mConstraintRow[k].m_normal);
+								{
-									float deltaImpulse = cp.mConstraintRow[k].m_accumImpulse;
+									cp.m_appliedImpulse,
+									cp.m_appliedImpulseLateral1,
+									cp.m_appliedImpulseLateral2
+								};
+								for(int k=0;k<3;k++) 
+								{
+									vmVector3 normal = btReadVector3(contactConstraintRows[j*3+k].m_normal);
+									contactConstraintRows[j*3+k].m_accumImpulse = imp[k];
+									float deltaImpulse = contactConstraintRows[j*3+k].m_accumImpulse;
 									solverBodyA.mDeltaLinearVelocity += deltaImpulse * solverBodyA.mMassInv * normal;
 									solverBodyA.mDeltaAngularVelocity += deltaImpulse * solverBodyA.mInertiaInv * cross(rA,normal);
 									solverBodyB.mDeltaLinearVelocity -= deltaImpulse * solverBodyB.mMassInv * normal;
@@ -246,9 +255,9 @@ void CustomSolveConstraintsTaskParallel(
 							}
 							else {
 								btSolveContactConstraint(
-									cp.mConstraintRow[0],
+									contactConstraintRows[j*3],
-									cp.mConstraintRow[1],
+									contactConstraintRows[j*3+1],
-									cp.mConstraintRow[2],
+									contactConstraintRows[j*3+2],
 									btReadVector3(cp.m_localPointA),
 									btReadVector3(cp.m_localPointB),
 									solverBodyA,
@@ -334,6 +343,11 @@ void btSetupContactConstraint(
 	const TrbState &stateB,
 	PfxSolverBody &solverBodyA,
 	PfxSolverBody &solverBodyB,
+	const vmVector3& linVelA, 
+	const vmVector3& angVelA,
+	const vmVector3& linVelB, 
+	const vmVector3& angVelB,
+
 	float separateBias,
 	float timeStep
 	)
@@ -345,10 +359,16 @@ void btSetupContactConstraint(
 			crossMatrix(rA) * solverBodyA.mInertiaInv * crossMatrix(rA) - 
 			crossMatrix(rB) * solverBodyB.mInertiaInv * crossMatrix(rB);
 	
+	//use the velocities without the applied (gravity and external) forces for restitution computation
+	vmVector3 vArestitution = linVelA + cross(angVelA,rA);
+	vmVector3 vBrestitution = linVelB + cross(angVelB,rB);
+	vmVector3 vABrestitution = vArestitution-vBrestitution;
+
 	vmVector3 vA = stateA.getLinearVelocity() + cross(stateA.getAngularVelocity(),rA);
 	vmVector3 vB = stateB.getLinearVelocity() + cross(stateB.getAngularVelocity(),rB);
 	vmVector3 vAB = vA-vB;
 
+
 	vmVector3 tangent1,tangent2;
 	btPlaneSpace1(contactNormal,tangent1,tangent2);
 
@@ -404,6 +424,7 @@ void btSetupContactConstraint(
 void CustomSetupContactConstraintsTask(
 	PfxConstraintPair *contactPairs,uint32_t numContactPairs,
 	btPersistentManifold*	offsetContactManifolds,
+	btConstraintRow* offsetContactConstraintRows,
 	TrbState *offsetRigStates,
 	PfxSolverBody *offsetSolverBodies,
 	uint32_t numRigidBodies,
@@ -422,7 +443,7 @@ void CustomSetupContactConstraintsTask(
 
 		int id = pfxGetConstraintId1(pair);
 		btPersistentManifold& contact = offsetContactManifolds[id];
-
+		btConstraintRow* contactConstraintRows = &offsetContactConstraintRows[id*12];
 
 		TrbState &stateA = offsetRigStates[iA];
 //		PfxRigBody &bodyA = offsetRigBodies[iA];
@@ -440,10 +461,39 @@ void CustomSetupContactConstraintsTask(
 		for(int j=0;j<contact.getNumContacts();j++) {
 			btManifoldPoint& cp = contact.getContactPoint(j);
 
+			//pass the velocities without the applied (gravity and external) forces for restitution computation
+			const btRigidBody* rbA = btRigidBody::upcast(contact.getBody0());
+			const btRigidBody* rbB = btRigidBody::upcast(contact.getBody1());
+
+			btVector3 linVelA, linVelB;
+			btVector3 angVelA, angVelB;
+			
+			if (rbA && (rbA->getInvMass()>0.f))
+			{
+				linVelA = rbA->getLinearVelocity();
+				angVelA = rbA->getAngularVelocity();
+			} else
+			{
+				linVelA.setValue(0,0,0);
+				angVelA.setValue(0,0,0);
+			}
+
+			if (rbB && (rbB->getInvMass()>0.f))
+			{
+				linVelB = rbB->getLinearVelocity();
+				angVelB = rbB->getAngularVelocity();
+			} else
+			{
+				linVelB.setValue(0,0,0);
+				angVelB.setValue(0,0,0);
+			}
+
+
+
 			btSetupContactConstraint(
-				cp.mConstraintRow[0],
+				contactConstraintRows[j*3],
-				cp.mConstraintRow[1],
+				contactConstraintRows[j*3+1],
-				cp.mConstraintRow[2],
+				contactConstraintRows[j*3+2],
 				cp.getDistance(),
 				restitution,
 				friction,
@@ -454,6 +504,8 @@ void CustomSetupContactConstraintsTask(
 				stateB,
 				solverBodyA,
 				solverBodyB,
+				(const vmVector3&)linVelA, (const vmVector3&)angVelA,
+				(const vmVector3&)linVelB, (const vmVector3&)angVelB,
 				separateBias,
 				timeStep
 				);
@@ -463,6 +515,36 @@ void CustomSetupContactConstraintsTask(
 	}
 }
 
+
+void CustomWritebackContactConstraintsTask(
+	PfxConstraintPair *contactPairs,uint32_t numContactPairs,
+	btPersistentManifold*	offsetContactManifolds,
+	btConstraintRow* offsetContactConstraintRows,
+	TrbState *offsetRigStates,
+	PfxSolverBody *offsetSolverBodies,
+	uint32_t numRigidBodies,
+	float separateBias,
+	float timeStep)
+{
+	for(uint32_t i=0;i<numContactPairs;i++) {
+		PfxConstraintPair &pair = contactPairs[i];
+		if(!pfxGetActive(pair) || pfxGetNumConstraints(pair) == 0 ||
+			((pfxGetMotionMaskA(pair)&PFX_MOTION_MASK_STATIC) && (pfxGetMotionMaskB(pair)&PFX_MOTION_MASK_STATIC)) ) {
+			continue;
+		}
+		int id = pfxGetConstraintId1(pair);
+		btPersistentManifold& contact = offsetContactManifolds[id];
+		btConstraintRow* contactConstraintRows = &offsetContactConstraintRows[id*12];
+		for(int j=0;j<contact.getNumContacts();j++) {
+			btManifoldPoint& cp = contact.getContactPoint(j);
+			cp.m_appliedImpulse = contactConstraintRows[j*3+0].m_accumImpulse;
+			cp.m_appliedImpulseLateral1 = contactConstraintRows[j*3+1].m_accumImpulse;
+			cp.m_appliedImpulseLateral2 = contactConstraintRows[j*3+2].m_accumImpulse;
+		}
+		//contact.setCompositeFriction(friction);
+	}
+}
+
 void	SolverThreadFunc(void* userPtr,void* lsMemory)
 {
 	btConstraintSolverIO* io = (btConstraintSolverIO*)(userPtr);//arg->io);
@@ -479,6 +561,7 @@ void	SolverThreadFunc(void* userPtr,void* lsMemory)
 			io->solveConstraints.contactPairs,
 			io->solveConstraints.numContactPairs,
 			io->solveConstraints.offsetContactManifolds,
+			io->solveConstraints.offsetContactConstraintRows,
 
 			io->solveConstraints.jointParallelGroup,
 			io->solveConstraints.jointParallelBatches,
@@ -528,6 +611,49 @@ void	SolverThreadFunc(void* userPtr,void* lsMemory)
 					CustomSetupContactConstraintsTask(
 						io->setupContactConstraints.offsetContactPairs+start,batch,
 						io->setupContactConstraints.offsetContactManifolds,
+						io->setupContactConstraints.offsetContactConstraintRows,
+						io->setupContactConstraints.offsetRigStates,
+//						io->setupContactConstraints.offsetRigBodies,
+						io->setupContactConstraints.offsetSolverBodies,
+						io->setupContactConstraints.numRigidBodies,
+						io->setupContactConstraints.separateBias,
+						io->setupContactConstraints.timeStep);
+				}
+				else {
+					empty = true;
+				}
+			}
+		}
+		break;
+
+		case PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS:
+		{
+			bool empty = false;
+			while(!empty) {
+				int start,batch;
+				
+				criticalsection->lock();
+
+				start = (int)criticalsection->getSharedParam(0);
+				batch = (int)criticalsection->getSharedParam(1);
+
+				//PFX_PRINTF("taskId %d start %d num %d\n",arg->taskId,start,batch);
+
+				// <20><><EFBFBD>̃o<CC83>b<EFBFBD>t<EFBFBD>@<40><><EFBFBD>Z<EFBFBD>b<EFBFBD>g
+				int nextStart = start + batch;
+				int rest = btMax((int)io->setupContactConstraints.numContactPairs1 - nextStart,0);
+				int nextBatch = (rest > batch)?batch:rest;
+
+				criticalsection->setSharedParam(0,nextStart);
+                criticalsection->setSharedParam(1,nextBatch);
+
+				criticalsection->unlock();
+				
+				if(batch > 0) {
+					CustomWritebackContactConstraintsTask(
+						io->setupContactConstraints.offsetContactPairs+start,batch,
+						io->setupContactConstraints.offsetContactManifolds,
+						io->setupContactConstraints.offsetContactConstraintRows,
 						io->setupContactConstraints.offsetRigStates,
 //						io->setupContactConstraints.offsetRigBodies,
 						io->setupContactConstraints.offsetSolverBodies,
@@ -554,6 +680,7 @@ void	SolverThreadFunc(void* userPtr,void* lsMemory)
 void CustomSetupContactConstraintsNew(
 	PfxConstraintPair *contactPairs1,uint32_t numContactPairs,
 	btPersistentManifold *offsetContactManifolds,
+	btConstraintRow* offsetContactConstraintRows,
 	TrbState *offsetRigStates,
 	PfxSolverBody *offsetSolverBodies,
 	uint32_t numRigidBodies,
@@ -561,7 +688,8 @@ void CustomSetupContactConstraintsNew(
 	float timeStep,
 	class btThreadSupportInterface* threadSupport,
 	btCriticalSection* criticalSection,
-	btConstraintSolverIO *io 
+	btConstraintSolverIO *io ,
+	uint8_t cmd
 	)
 {
 	int maxTasks = threadSupport->getNumTasks();
@@ -584,11 +712,12 @@ void CustomSetupContactConstraintsNew(
 	}
 
 	for(int t=0;t<maxTasks;t++) {
-		io[t].cmd = PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS;
+		io[t].cmd = cmd;
 		io[t].setupContactConstraints.offsetContactPairs = contactPairs1;
 		io[t].setupContactConstraints.numContactPairs1 = numContactPairs;
 		io[t].setupContactConstraints.offsetRigStates = offsetRigStates;
 		io[t].setupContactConstraints.offsetContactManifolds = offsetContactManifolds;	
+		io[t].setupContactConstraints.offsetContactConstraintRows = offsetContactConstraintRows;
 		io[t].setupContactConstraints.offsetSolverBodies = offsetSolverBodies;
 		io[t].setupContactConstraints.numRigidBodies = numRigidBodies;
 		io[t].setupContactConstraints.separateBias = separationBias;
@@ -733,6 +862,7 @@ void CustomSolveConstraintsParallel(
 	
 	PfxConstraintPair *jointPairs,uint32_t numJointPairs,
 	btPersistentManifold* offsetContactManifolds,
+	btConstraintRow* offsetContactConstraintRows,
 	btSolverConstraint* offsetSolverConstraints,
 	TrbState *offsetRigStates,
 	PfxSolverBody *offsetSolverBodies,
@@ -780,9 +910,9 @@ void CustomSolveConstraintsParallel(
 			io->solveConstraints.jointParallelBatches,
 			io->solveConstraints.jointPairs,
 			io->solveConstraints.numJointPairs,
-			io->solveConstraints.offsetJoints,
+			io->solveConstraints.offsetSolverConstraints,
 
-			io->solveConstraints.offsetRigStates,
+			io->solveConstraints.offsetRigStates1,
 			io->solveConstraints.offsetSolverBodies,
 			io->solveConstraints.numRigidBodies,
 			io->solveConstraints.iteration,0,1,0);//arg->taskId,1,0);//,arg->maxTasks,arg->barrier);
@@ -794,6 +924,7 @@ void CustomSolveConstraintsParallel(
 			io[t].solveConstraints.contactPairs = contactPairs;
 			io[t].solveConstraints.numContactPairs = numContactPairs;
 			io[t].solveConstraints.offsetContactManifolds = offsetContactManifolds;
+			io[t].solveConstraints.offsetContactConstraintRows = offsetContactConstraintRows;
 			io[t].solveConstraints.jointParallelGroup = jgroup;
 			io[t].solveConstraints.jointParallelBatches = jbatches;
 			io[t].solveConstraints.jointPairs = jointPairs;
@@ -869,6 +1000,7 @@ void CustomSolveConstraintsParallel(
 
 void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphasePair *new_pairs1 ,
 									btPersistentManifold* offsetContactManifolds,
+									PfxConstraintRow*	offsetContactConstraintRows,
 									  TrbState* states,int numRigidBodies, 
 									  struct PfxSolverBody* solverBodies, 
 									  PfxConstraintPair* jointPairs, unsigned int numJoints,
@@ -920,16 +1052,19 @@ void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphase
 			separateBias,
 			timeStep);
 #else
+
 		CustomSetupContactConstraintsNew(
 			(PfxConstraintPair*)new_pairs1,new_num,
 			offsetContactManifolds,
+			offsetContactConstraintRows,
 			states,
 			solverBodies,
 			numRigidBodies,
 			separateBias,
 			timeStep,
 			solverThreadSupport,
-			criticalSection,solverIO
+			criticalSection,solverIO,
+			PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS
 			);
 
 #endif //SEQUENTIAL_SETUP
@@ -954,6 +1089,7 @@ void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphase
 			(PfxConstraintPair*)new_pairs1,new_num,
 			jointPairs,numJoints,
 			offsetContactManifolds,
+			offsetContactConstraintRows,
 			offsetSolverConstraints,
 			states,
 			solverBodies,
@@ -968,6 +1104,24 @@ void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphase
 #endif //SEQUENTIAL
 	}
 
+	{
+		BT_PROFILE("writeback appliedImpulses");
+
+		CustomSetupContactConstraintsNew(
+			(PfxConstraintPair*)new_pairs1,new_num,
+			offsetContactManifolds,
+			offsetContactConstraintRows,
+			states,
+			solverBodies,
+			numRigidBodies,
+			separateBias,
+			timeStep,
+			solverThreadSupport,
+			criticalSection,solverIO,
+			PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS
+			);
+	}
+
 }
 
 
@@ -977,6 +1131,7 @@ struct	btParallelSolverMemoryCache
 	btAlignedObjectArray<PfxSolverBody>  m_mysolverbodies;
 	btAlignedObjectArray<PfxBroadphasePair> m_mypairs;
 	btAlignedObjectArray<PfxConstraintPair> m_jointPairs;
+	btAlignedObjectArray<PfxConstraintRow> m_constraintRows;
 	
 };
 
@@ -1057,8 +1212,11 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int
 		//if(state.getMotionMask()&PFX_MOTION_MASK_DYNAMIC) {
 		if (rb && (rb->getInvMass()>0.f))
 		{
-			state.setAngularVelocity(vmVector3(rb->getAngularVelocity().getX(),rb->getAngularVelocity().getY(),rb->getAngularVelocity().getZ()));
+			btVector3 angVelPlusForces = rb->getAngularVelocity()+rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
-			state.setLinearVelocity(vmVector3(rb->getLinearVelocity().getX(),rb->getLinearVelocity().getY(),rb->getLinearVelocity().getZ()));
+			btVector3 linVelPlusForces = rb->getLinearVelocity()+rb->getTotalForce()*rb->getInvMass()*infoGlobal.m_timeStep;
+
+			state.setAngularVelocity((const vmVector3&)angVelPlusForces);
+			state.setLinearVelocity((const vmVector3&) linVelPlusForces);
 
 			state.setMotionType(PfxMotionTypeActive);
 			vmMatrix3 ori(solverBody.mOrientation);
@@ -1087,6 +1245,8 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int
 	int totalPoints = 0;
 #ifndef USE_C_ARRAYS
 	m_memoryCache->m_mypairs.resize(numManifolds);
+	//4 points per manifold and 3 rows per point makes 12 rows per manifold
+	m_memoryCache->m_constraintRows.resize(numManifolds*12);
 	m_memoryCache->m_jointPairs.resize(numConstraints);
 #endif//USE_C_ARRAYS
 
@@ -1197,6 +1357,9 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int
 					btRigidBody& rbA = constraint->getRigidBodyA();
 					btRigidBody& rbB = constraint->getRigidBodyB();
 			
+					int idA = constraint->getRigidBodyA().getCompanionId();
+					int idB = constraint->getRigidBodyB().getCompanionId();
+			
 					
 					int j;
 					for ( j=0;j<info1.m_numConstraintRows;j++)
@@ -1206,14 +1369,11 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int
 						currentConstraintRow[j].m_upperLimit = FLT_MAX;
 						currentConstraintRow[j].m_appliedImpulse = 0.f;
 						currentConstraintRow[j].m_appliedPushImpulse = 0.f;
-						currentConstraintRow[j].m_solverBodyA = &rbA;
+						currentConstraintRow[j].m_solverBodyIdA = idA;
-						currentConstraintRow[j].m_solverBodyB = &rbB;
+						currentConstraintRow[j].m_solverBodyIdB = idB;
 					}
 
-					rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
+				
-					rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
-					rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
-					rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
 
 
 
@@ -1236,8 +1396,6 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int
 					constraints[i]->getInfo2(&info2);
 
 					
-					int idA = constraint->getRigidBodyA().getCompanionId();
-					int idB = constraint->getRigidBodyB().getCompanionId();
 				
 
 					///finalize the constraint setup
@@ -1246,8 +1404,8 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int
 						btSolverConstraint& solverConstraint = currentConstraintRow[j];
 						solverConstraint.m_originalContactPoint = constraint;
 
-						solverConstraint.m_companionIdA = idA;
+						solverConstraint.m_solverBodyIdA = idA;
-						solverConstraint.m_companionIdB = idB;
+						solverConstraint.m_solverBodyIdB = idB;
 
 						{
 							const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
@@ -1356,11 +1514,13 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int
 //			PFX_PRINTF("num points PFX = %d\n",total);
 			
 			
-			 
+			PfxConstraintRow* contactRows = actualNumManifolds? &m_memoryCache->m_constraintRows[0] : 0;
+			 PfxBroadphasePair* actualPairs = m_memoryCache->m_mypairs.size() ? &m_memoryCache->m_mypairs[0] : 0;
 			BPE_customConstraintSolverSequentialNew(
 				actualNumManifolds,
-				&m_memoryCache->m_mypairs[0],
+				actualPairs,
 				offsetContactManifolds,
+				contactRows,
 				&m_memoryCache->m_mystates[0],numRigidBodies,
 				&m_memoryCache->m_mysolverbodies[0],
 				jointPairs,actualNumJoints,

src/BulletMultiThreaded/btParallelConstraintSolver.h

@@ -188,6 +188,7 @@ class btPersistentManifold;
 enum {
 	PFX_CONSTRAINT_SOLVER_CMD_SETUP_SOLVER_BODIES,
 	PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS,
+	PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS,
 	PFX_CONSTRAINT_SOLVER_CMD_SETUP_JOINT_CONSTRAINTS,
 	PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS,
 	PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER
@@ -198,6 +199,7 @@ struct PfxSetupContactConstraintsIO {
 	PfxConstraintPair *offsetContactPairs;
 	uint32_t numContactPairs1;
 	btPersistentManifold*	offsetContactManifolds;
+	btConstraintRow* offsetContactConstraintRows;
 	class TrbState *offsetRigStates;
 	struct PfxSolverBody *offsetSolverBodies;
 	uint32_t numRigidBodies;
@@ -214,6 +216,7 @@ struct PfxSolveConstraintsIO {
 	PfxConstraintPair *contactPairs;
 	uint32_t numContactPairs;
 	btPersistentManifold *offsetContactManifolds;
+	btConstraintRow*	offsetContactConstraintRows;
 	PfxParallelGroup *jointParallelGroup;
 	PfxParallelBatch *jointParallelBatches;
 	PfxConstraintPair *jointPairs;