Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp	(revision 8284)
@@ -23,16 +23,18 @@
 #include <new>
 
-//-----------------------------------------------------------------------------
-
+
+
+//#define CONETWIST_USE_OBSOLETE_SOLVER true
 #define CONETWIST_USE_OBSOLETE_SOLVER false
 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
 
-//-----------------------------------------------------------------------------
-
-btConeTwistConstraint::btConeTwistConstraint()
-:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
-m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
-{
-}
+
+SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
+{
+	btVector3 vec = axis * invInertiaWorld;
+	return axis.dot(vec);
+}
+
+
 
 
@@ -64,11 +66,13 @@
 	m_maxMotorImpulse = btScalar(-1);
 
-	setLimit(btScalar(1e30), btScalar(1e30), btScalar(1e30));
+	setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
 	m_damping = btScalar(0.01);
 	m_fixThresh = CONETWIST_DEF_FIX_THRESH;
-}
-
-
-//-----------------------------------------------------------------------------
+	m_flags = 0;
+	m_linCFM = btScalar(0.f);
+	m_linERP = btScalar(0.7f);
+	m_angCFM = btScalar(0.f);
+}
+
 
 void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
@@ -83,5 +87,5 @@
 		info->m_numConstraintRows = 3;
 		info->nub = 3;
-		calcAngleInfo2();
+		calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
 		if(m_solveSwingLimit)
 		{
@@ -100,21 +104,30 @@
 		}
 	}
-} // btConeTwistConstraint::getInfo1()
+}
+
+void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+{
+	//always reserve 6 rows: object transform is not available on SPU
+	info->m_numConstraintRows = 6;
+	info->nub = 0;
+		
+}
 	
-//-----------------------------------------------------------------------------
 
 void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
 {
+	getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
+}
+
+void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
+{
+	calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB);
+	
 	btAssert(!m_useSolveConstraintObsolete);
-	//retrieve matrices
-	btTransform body0_trans;
-	body0_trans = m_rbA.getCenterOfMassTransform();
-    btTransform body1_trans;
-	body1_trans = m_rbB.getCenterOfMassTransform();
     // set jacobian
     info->m_J1linearAxis[0] = 1;
     info->m_J1linearAxis[info->rowskip+1] = 1;
     info->m_J1linearAxis[2*info->rowskip+2] = 1;
-	btVector3 a1 = body0_trans.getBasis() * m_rbAFrame.getOrigin();
+	btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin();
 	{
 		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
@@ -124,5 +137,5 @@
 		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
 	}
-	btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin();
+	btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin();
 	{
 		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
@@ -132,11 +145,16 @@
 	}
     // set right hand side
-    btScalar k = info->fps * info->erp;
+	btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
+    btScalar k = info->fps * linERP;
     int j;
 	for (j=0; j<3; j++)
     {
-        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
+        info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
 		info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
 		info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
+		if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
+		{
+			info->cfm[j*info->rowskip] = m_linCFM;
+		}
     }
 	int row = 3;
@@ -150,5 +168,5 @@
 		if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
 		{
-			btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
+			btTransform trA = transA*m_rbAFrame;
 			btVector3 p = trA.getBasis().getColumn(1);
 			btVector3 q = trA.getBasis().getColumn(2);
@@ -187,5 +205,8 @@
 
 			info->m_constraintError[srow] = k * m_swingCorrection;
-			info->cfm[srow] = 0.0f;
+			if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
+			{
+				info->cfm[srow] = m_angCFM;
+			}
 			// m_swingCorrection is always positive or 0
 			info->m_lowerLimit[srow] = 0;
@@ -207,5 +228,8 @@
 		btScalar k = info->fps * m_biasFactor;
 		info->m_constraintError[srow] = k * m_twistCorrection;
-		info->cfm[srow] = 0.0f;
+		if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
+		{
+			info->cfm[srow] = m_angCFM;
+		}
 		if(m_twistSpan > 0.0f)
 		{
@@ -231,5 +255,5 @@
 }
 	
-//-----------------------------------------------------------------------------
+
 
 void	btConeTwistConstraint::buildJacobian()
@@ -240,4 +264,5 @@
 		m_accTwistLimitImpulse = btScalar(0.);
 		m_accSwingLimitImpulse = btScalar(0.);
+		m_accMotorImpulse = btVector3(0.,0.,0.);
 
 		if (!m_angularOnly)
@@ -274,12 +299,13 @@
 		}
 
-		calcAngleInfo2();
-	}
-}
-
-//-----------------------------------------------------------------------------
-
-void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
-{
+		calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
+	}
+}
+
+
+
+void	btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar	timeStep)
+{
+	#ifndef __SPU__
 	if (m_useSolveConstraintObsolete)
 	{
@@ -296,7 +322,7 @@
 
 			btVector3 vel1;
-			bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+			bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
 			btVector3 vel2;
-			bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+			bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
 			btVector3 vel = vel1 - vel2;
 
@@ -315,6 +341,6 @@
 				btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
 				btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
-				bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
-				bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
+				bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
+				bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
 		
 			}
@@ -327,6 +353,6 @@
 			btTransform trACur = m_rbA.getCenterOfMassTransform();
 			btTransform trBCur = m_rbB.getCenterOfMassTransform();
-			btVector3 omegaA; bodyA.getAngularVelocity(omegaA);
-			btVector3 omegaB; bodyB.getAngularVelocity(omegaB);
+			btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
+			btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
 			btTransform trAPred; trAPred.setIdentity(); 
 			btVector3 zerovec(0,0,0);
@@ -402,13 +428,13 @@
 				btVector3 impulseAxis =  impulse / impulseMag;
 
-				bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
-				bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
-
-			}
-		}
-		else // no motor: do a little damping
-		{
-			const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
-			const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
+				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
+				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+
+			}
+		}
+		else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
+		{
+			btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
+			btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
 			btVector3 relVel = angVelB - angVelA;
 			if (relVel.length2() > SIMD_EPSILON)
@@ -422,6 +448,6 @@
 				btScalar  impulseMag  = impulse.length();
 				btVector3 impulseAxis = impulse / impulseMag;
-				bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
-				bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
+				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
 			}
 		}
@@ -431,7 +457,7 @@
 			///solve angular part
 			btVector3 angVelA;
-			bodyA.getAngularVelocity(angVelA);
+			bodyA.internalGetAngularVelocity(angVelA);
 			btVector3 angVelB;
-			bodyB.getAngularVelocity(angVelB);
+			bodyB.internalGetAngularVelocity(angVelB);
 
 			// solve swing limit
@@ -462,6 +488,6 @@
 				btVector3 noTwistSwingAxis = impulse / impulseMag;
 
-				bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
-				bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
+				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
+				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
 			}
 
@@ -483,13 +509,16 @@
 				btVector3 impulse = m_twistAxis * impulseMag;
 
-				bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
-				bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
+				bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
+				bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
 			}		
 		}
 	}
-
-}
-
-//-----------------------------------------------------------------------------
+#else
+btAssert(0);
+#endif //__SPU__
+}
+
+
+
 
 void	btConeTwistConstraint::updateRHS(btScalar	timeStep)
@@ -499,6 +528,6 @@
 }
 
-//-----------------------------------------------------------------------------
-
+
+#ifndef __SPU__
 void btConeTwistConstraint::calcAngleInfo()
 {
@@ -585,12 +614,12 @@
 		}
 	}
-} // btConeTwistConstraint::calcAngleInfo()
-
+}
+#endif //__SPU__
 
 static btVector3 vTwist(1,0,0); // twist axis in constraint's space
 
-//-----------------------------------------------------------------------------
-
-void btConeTwistConstraint::calcAngleInfo2()
+
+
+void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
 {
 	m_swingCorrection = btScalar(0.);
@@ -598,11 +627,32 @@
 	m_solveTwistLimit = false;
 	m_solveSwingLimit = false;
+	// compute rotation of A wrt B (in constraint space)
+	if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
+	{	// it is assumed that setMotorTarget() was alredy called 
+		// and motor target m_qTarget is within constraint limits
+		// TODO : split rotation to pure swing and pure twist
+		// compute desired transforms in world
+		btTransform trPose(m_qTarget);
+		btTransform trA = transA * m_rbAFrame;
+		btTransform trB = transB * m_rbBFrame;
+		btTransform trDeltaAB = trB * trPose * trA.inverse();
+		btQuaternion qDeltaAB = trDeltaAB.getRotation();
+		btVector3 swingAxis = 	btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
+		m_swingAxis = swingAxis;
+		m_swingAxis.normalize();
+		m_swingCorrection = qDeltaAB.getAngle();
+		if(!btFuzzyZero(m_swingCorrection))
+		{
+			m_solveSwingLimit = true;
+		}
+		return;
+	}
+
 
 	{
 		// compute rotation of A wrt B (in constraint space)
-		btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation();
-		btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation();
+		btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
+		btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
 		btQuaternion qAB = qB.inverse() * qA;
-
 		// split rotation into cone and twist
 		// (all this is done from B's perspective. Maybe I should be averaging axes...)
@@ -642,6 +692,6 @@
 
 				m_kSwing =  btScalar(1.) /
-					(getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
-					 getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
+					(computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
+					 computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
 			}
 		}
@@ -651,8 +701,8 @@
 			// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
 			// anyway, we have either hinge or fixed joint
-			btVector3 ivA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
-			btVector3 jvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
-			btVector3 kvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
-			btVector3 ivB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(0);
+			btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
+			btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
+			btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
+			btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
 			btVector3 target;
 			btScalar x = ivB.dot(ivA);
@@ -745,6 +795,6 @@
 
 				m_kTwist = btScalar(1.) /
-					(getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
-					 getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
+					(computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
+					 computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
 			}
 
@@ -757,5 +807,5 @@
 		}
 	}
-} // btConeTwistConstraint::calcAngleInfo2()
+}
 
 
@@ -982,8 +1032,86 @@
 }
 
-
-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
-
-
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+///If no axis is provided, it uses the default axis for this constraint.
+void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
+{
+	switch(num)
+	{
+		case BT_CONSTRAINT_ERP :
+		case BT_CONSTRAINT_STOP_ERP :
+			if((axis >= 0) && (axis < 3)) 
+			{
+				m_linERP = value;
+				m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
+			}
+			else
+			{
+				m_biasFactor = value;
+			}
+			break;
+		case BT_CONSTRAINT_CFM :
+		case BT_CONSTRAINT_STOP_CFM :
+			if((axis >= 0) && (axis < 3)) 
+			{
+				m_linCFM = value;
+				m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
+			}
+			else
+			{
+				m_angCFM = value;
+				m_flags |= BT_CONETWIST_FLAGS_ANG_CFM;
+			}
+			break;
+		default:
+			btAssertConstrParams(0);
+			break;
+	}
+}
+
+///return the local value of parameter
+btScalar btConeTwistConstraint::getParam(int num, int axis) const 
+{
+	btScalar retVal = 0;
+	switch(num)
+	{
+		case BT_CONSTRAINT_ERP :
+		case BT_CONSTRAINT_STOP_ERP :
+			if((axis >= 0) && (axis < 3)) 
+			{
+				btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
+				retVal = m_linERP;
+			}
+			else if((axis >= 3) && (axis < 6)) 
+			{
+				retVal = m_biasFactor;
+			}
+			else
+			{
+				btAssertConstrParams(0);
+			}
+			break;
+		case BT_CONSTRAINT_CFM :
+		case BT_CONSTRAINT_STOP_CFM :
+			if((axis >= 0) && (axis < 3)) 
+			{
+				btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
+				retVal = m_linCFM;
+			}
+			else if((axis >= 3) && (axis < 6)) 
+			{
+				btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
+				retVal = m_angCFM;
+			}
+			else
+			{
+				btAssertConstrParams(0);
+			}
+			break;
+		default : 
+			btAssertConstrParams(0);
+	}
+	return retVal;
+}
+
+
+
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h	(revision 8284)
@@ -18,4 +18,20 @@
 
 
+/*
+Overview:
+
+btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc).
+It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint".
+It divides the 3 rotational DOFs into swing (movement within a cone) and twist.
+Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape.
+(Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.)
+
+In the contraint's frame of reference:
+twist is along the x-axis,
+and swing 1 and 2 are along the z and y axes respectively.
+*/
+
+
+
 #ifndef CONETWISTCONSTRAINT_H
 #define CONETWISTCONSTRAINT_H
@@ -27,4 +43,10 @@
 class btRigidBody;
 
+enum btConeTwistFlags
+{
+	BT_CONETWIST_FLAGS_LIN_CFM = 1,
+	BT_CONETWIST_FLAGS_LIN_ERP = 2,
+	BT_CONETWIST_FLAGS_ANG_CFM = 4
+};
 
 ///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
@@ -84,4 +106,23 @@
 	btVector3	 m_accMotorImpulse;
 	
+	// parameters
+	int			m_flags;
+	btScalar	m_linCFM;
+	btScalar	m_linERP;
+	btScalar	m_angCFM;
+	
+protected:
+
+	void init();
+
+	void computeConeLimitInfo(const btQuaternion& qCone, // in
+		btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
+
+	void computeTwistLimitInfo(const btQuaternion& qTwist, // in
+		btScalar& twistAngle, btVector3& vTwistAxis); // all outs
+
+	void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
+
+
 public:
 
@@ -90,14 +131,15 @@
 	btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
 
-	btConeTwistConstraint();
-
 	virtual void	buildJacobian();
 
 	virtual void getInfo1 (btConstraintInfo1* info);
+
+	void	getInfo1NonVirtual(btConstraintInfo1* info);
 	
 	virtual void getInfo2 (btConstraintInfo2* info);
 	
-
-	virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
+	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);
 
 	void	updateRHS(btScalar	timeStep);
@@ -142,5 +184,16 @@
 	}
 
-	void	setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+	// setLimit(), a few notes:
+	// _softness:
+	//		0->1, recommend ~0.8->1.
+	//		describes % of limits where movement is free.
+	//		beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached.
+	// _biasFactor:
+	//		0->1?, recommend 0.3 +/-0.3 or so.
+	//		strength with which constraint resists zeroth order (angular, not angular velocity) limit violation.
+	// __relaxationFactor:
+	//		0->1, recommend to stay near 1.
+	//		the lower the value, the less the constraint will fight velocities which violate the angular limits.
+	void	setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
 	{
 		m_swingSpan1 = _swingSpan1;
@@ -172,5 +225,5 @@
 
 	void calcAngleInfo();
-	void calcAngleInfo2();
+	void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
 
 	inline btScalar getSwingSpan1()
@@ -213,17 +266,67 @@
 	btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
 
-
-
-protected:
-	void init();
-
-	void computeConeLimitInfo(const btQuaternion& qCone, // in
-		btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
-
-	void computeTwistLimitInfo(const btQuaternion& qTwist, // in
-		btScalar& twistAngle, btVector3& vTwistAxis); // all outs
-
-	void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///If no axis is provided, it uses the default axis for this constraint.
+	virtual	void setParam(int num, btScalar value, int axis = -1);
+	///return the local value of parameter
+	virtual	btScalar getParam(int num, int axis = -1) const;
+
+	virtual	int	calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+
 };
 
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btConeTwistConstraintData
+{
+	btTypedConstraintData	m_typeConstraintData;
+	btTransformFloatData m_rbAFrame;
+	btTransformFloatData m_rbBFrame;
+
+	//limits
+	float	m_swingSpan1;
+	float	m_swingSpan2;
+	float	m_twistSpan;
+	float	m_limitSoftness;
+	float	m_biasFactor;
+	float	m_relaxationFactor;
+
+	float	m_damping;
+		
+	char m_pad[4];
+
+};
+	
+
+
+SIMD_FORCE_INLINE int	btConeTwistConstraint::calculateSerializeBufferSize() const
+{
+	return sizeof(btConeTwistConstraintData);
+
+}
+
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char*	btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btConeTwistConstraintData* cone = (btConeTwistConstraintData*) dataBuffer;
+	btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
+
+	m_rbAFrame.serializeFloat(cone->m_rbAFrame);
+	m_rbBFrame.serializeFloat(cone->m_rbBFrame);
+	
+	cone->m_swingSpan1 = float(m_swingSpan1);
+	cone->m_swingSpan2 = float(m_swingSpan2);
+	cone->m_twistSpan = float(m_twistSpan);
+	cone->m_limitSoftness = float(m_limitSoftness);
+	cone->m_biasFactor = float(m_biasFactor);
+	cone->m_relaxationFactor = float(m_relaxationFactor);
+	cone->m_damping = float(m_damping);
+
+	return "btConeTwistConstraintData";
+}
+
+
 #endif //CONETWISTCONSTRAINT_H
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp	(revision 8284)
@@ -13,4 +13,50 @@
 3. This notice may not be removed or altered from any source distribution.
 */
+
+
+#include "btContactConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btContactSolverInfo.h"
+#include "LinearMath/btMinMax.h"
+#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
+
+
+
+btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB)
+:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB),
+	m_contactManifold(*contactManifold)
+{
+
+}
+
+btContactConstraint::~btContactConstraint()
+{
+
+}
+
+void	btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
+{
+	m_contactManifold = *contactManifold;
+}
+
+void btContactConstraint::getInfo1 (btConstraintInfo1* info)
+{
+
+}
+
+void btContactConstraint::getInfo2 (btConstraintInfo2* info)
+{
+
+}
+
+void	btContactConstraint::buildJacobian()
+{
+
+}
+
+
+
 
 
@@ -86,344 +132,3 @@
 
 
-//response  between two dynamic objects with friction
-btScalar resolveSingleCollision(
-	btRigidBody& body1,
-	btRigidBody& body2,
-	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo)
-{
 
-	const btVector3& pos1_ = contactPoint.getPositionWorldOnA();
-	const btVector3& pos2_ = contactPoint.getPositionWorldOnB();
-   	const btVector3& normal = contactPoint.m_normalWorldOnB;
-
-	//constant over all iterations
-	btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition(); 
-	btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition();
-	
-	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-	btVector3 vel = vel1 - vel2;
-	btScalar rel_vel;
-	rel_vel = normal.dot(vel);
-	
-	btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
-
-	// btScalar damping = solverInfo.m_damping ;
-	btScalar Kerp = solverInfo.m_erp;
-	btScalar Kcor = Kerp *Kfps;
-
-	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-	btAssert(cpd);
-	btScalar distance = cpd->m_penetration;
-	btScalar positionalError = Kcor *-distance;
-	btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
-
-	btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
-
-	btScalar	velocityImpulse = velocityError * cpd->m_jacDiagABInv;
-
-	btScalar normalImpulse = penetrationImpulse+velocityImpulse;
-	
-	// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-	btScalar oldNormalImpulse = cpd->m_appliedImpulse;
-	btScalar sum = oldNormalImpulse + normalImpulse;
-	cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-
-	normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
-
-#ifdef USE_INTERNAL_APPLY_IMPULSE
-	if (body1.getInvMass())
-	{
-		body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
-	}
-	if (body2.getInvMass())
-	{
-		body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
-	}
-#else //USE_INTERNAL_APPLY_IMPULSE
-	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
-	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
-#endif //USE_INTERNAL_APPLY_IMPULSE
-
-	return normalImpulse;
-}
-
-
-btScalar resolveSingleFriction(
-	btRigidBody& body1,
-	btRigidBody& body2,
-	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo)
-{
-
-	(void)solverInfo;
-
-	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
-	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
-
-	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
-	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
-	
-	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-	btAssert(cpd);
-
-	btScalar combinedFriction = cpd->m_friction;
-	
-	btScalar limit = cpd->m_appliedImpulse * combinedFriction;
-	
-	if (cpd->m_appliedImpulse>btScalar(0.))
-	//friction
-	{
-		//apply friction in the 2 tangential directions
-		
-		// 1st tangent
-		btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-		btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-		btVector3 vel = vel1 - vel2;
-	
-		btScalar j1,j2;
-
-		{
-				
-			btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
-
-			// calculate j that moves us to zero relative velocity
-			j1 = -vrel * cpd->m_jacDiagABInvTangent0;
-			btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;
-			cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;
-			btSetMin(cpd->m_accumulatedTangentImpulse0, limit);
-			btSetMax(cpd->m_accumulatedTangentImpulse0, -limit);
-			j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;
-
-		}
-		{
-			// 2nd tangent
-
-			btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
-			
-			// calculate j that moves us to zero relative velocity
-			j2 = -vrel * cpd->m_jacDiagABInvTangent1;
-			btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;
-			cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;
-			btSetMin(cpd->m_accumulatedTangentImpulse1, limit);
-			btSetMax(cpd->m_accumulatedTangentImpulse1, -limit);
-			j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;
-		}
-
-#ifdef USE_INTERNAL_APPLY_IMPULSE
-	if (body1.getInvMass())
-	{
-		body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
-		body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
-	}
-	if (body2.getInvMass())
-	{
-		body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
-		body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);	
-	}
-#else //USE_INTERNAL_APPLY_IMPULSE
-	body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
-	body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
-#endif //USE_INTERNAL_APPLY_IMPULSE
-
-
-	} 
-	return cpd->m_appliedImpulse;
-}
-
-
-btScalar resolveSingleFrictionOriginal(
-	btRigidBody& body1,
-	btRigidBody& body2,
-	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo);
-
-btScalar resolveSingleFrictionOriginal(
-	btRigidBody& body1,
-	btRigidBody& body2,
-	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo)
-{
-
-	(void)solverInfo;
-
-	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
-	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
-
-	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
-	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
-	
-	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-	btAssert(cpd);
-
-	btScalar combinedFriction = cpd->m_friction;
-	
-	btScalar limit = cpd->m_appliedImpulse * combinedFriction;
-	//if (contactPoint.m_appliedImpulse>btScalar(0.))
-	//friction
-	{
-		//apply friction in the 2 tangential directions
-		
-		{
-			// 1st tangent
-			btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-			btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-			btVector3 vel = vel1 - vel2;
-			
-			btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
-
-			// calculate j that moves us to zero relative velocity
-			btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
-			btScalar total = cpd->m_accumulatedTangentImpulse0 + j;
-			btSetMin(total, limit);
-			btSetMax(total, -limit);
-			j = total - cpd->m_accumulatedTangentImpulse0;
-			cpd->m_accumulatedTangentImpulse0 = total;
-			body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
-			body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
-		}
-
-				
-		{
-			// 2nd tangent
-			btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-			btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-			btVector3 vel = vel1 - vel2;
-
-			btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
-			
-			// calculate j that moves us to zero relative velocity
-			btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
-			btScalar total = cpd->m_accumulatedTangentImpulse1 + j;
-			btSetMin(total, limit);
-			btSetMax(total, -limit);
-			j = total - cpd->m_accumulatedTangentImpulse1;
-			cpd->m_accumulatedTangentImpulse1 = total;
-			body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
-			body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
-		}
-	} 
-	return cpd->m_appliedImpulse;
-}
-
-
-//velocity + friction
-//response  between two dynamic objects with friction
-btScalar resolveSingleCollisionCombined(
-	btRigidBody& body1,
-	btRigidBody& body2,
-	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo)
-{
-
-	const btVector3& pos1 = contactPoint.getPositionWorldOnA();
-	const btVector3& pos2 = contactPoint.getPositionWorldOnB();
-   	const btVector3& normal = contactPoint.m_normalWorldOnB;
-
-	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
-	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
-	
-	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-	btVector3 vel = vel1 - vel2;
-	btScalar rel_vel;
-	rel_vel = normal.dot(vel);
-	
-	btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
-
-	//btScalar damping = solverInfo.m_damping ;
-	btScalar Kerp = solverInfo.m_erp;
-	btScalar Kcor = Kerp *Kfps;
-
-	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-	btAssert(cpd);
-	btScalar distance = cpd->m_penetration;
-	btScalar positionalError = Kcor *-distance;
-	btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
-
-	btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
-
-	btScalar	velocityImpulse = velocityError * cpd->m_jacDiagABInv;
-
-	btScalar normalImpulse = penetrationImpulse+velocityImpulse;
-	
-	// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-	btScalar oldNormalImpulse = cpd->m_appliedImpulse;
-	btScalar sum = oldNormalImpulse + normalImpulse;
-	cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-
-	normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
-
-
-#ifdef USE_INTERNAL_APPLY_IMPULSE
-	if (body1.getInvMass())
-	{
-		body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
-	}
-	if (body2.getInvMass())
-	{
-		body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
-	}
-#else //USE_INTERNAL_APPLY_IMPULSE
-	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
-	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
-#endif //USE_INTERNAL_APPLY_IMPULSE
-
-	{
-		//friction
-		btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-		btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-		btVector3 vel = vel1 - vel2;
-	  
-		rel_vel = normal.dot(vel);
-
-
-		btVector3 lat_vel = vel - normal * rel_vel;
-		btScalar lat_rel_vel = lat_vel.length();
-
-		btScalar combinedFriction = cpd->m_friction;
-
-		if (cpd->m_appliedImpulse > 0)
-		if (lat_rel_vel > SIMD_EPSILON)
-		{
-			lat_vel /= lat_rel_vel;
-			btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
-			btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
-			btScalar friction_impulse = lat_rel_vel /
-				(body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
-			btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
-
-			btSetMin(friction_impulse, normal_impulse);
-			btSetMax(friction_impulse, -normal_impulse);
-			body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
-			body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
-		}
-	}
-
-
-
-	return normalImpulse;
-}
-
-btScalar resolveSingleFrictionEmpty(
-	btRigidBody& body1,
-	btRigidBody& body2,
-	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo);
-
-btScalar resolveSingleFrictionEmpty(
-	btRigidBody& body1,
-	btRigidBody& body2,
-	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo)
-{
-	(void)contactPoint;
-	(void)body1;
-	(void)body2;
-	(void)solverInfo;
-
-
-	return btScalar(0.);
-}
-
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.h	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.h	(revision 8284)
@@ -17,77 +17,47 @@
 #define CONTACT_CONSTRAINT_H
 
-///@todo: make into a proper class working with the iterative constraint solver
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 
-class btRigidBody;
-#include "LinearMath/btVector3.h"
-#include "LinearMath/btScalar.h"
-struct btContactSolverInfo;
-class btManifoldPoint;
+///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface
+ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint
+{
+protected:
 
-enum {
-	DEFAULT_CONTACT_SOLVER_TYPE=0,
-	CONTACT_SOLVER_TYPE1,
-	CONTACT_SOLVER_TYPE2,
-	USER_CONTACT_SOLVER_TYPE1,
-	MAX_CONTACT_SOLVER_TYPES
+	btPersistentManifold m_contactManifold;
+
+public:
+
+
+	btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB);
+
+	void	setContactManifold(btPersistentManifold* contactManifold);
+
+	btPersistentManifold* getContactManifold()
+	{
+		return &m_contactManifold;
+	}
+
+	const btPersistentManifold* getContactManifold() const
+	{
+		return &m_contactManifold;
+	}
+
+	virtual ~btContactConstraint();
+
+	virtual void getInfo1 (btConstraintInfo1* info);
+
+	virtual void getInfo2 (btConstraintInfo2* info);
+
+	///obsolete methods
+	virtual void	buildJacobian();
+
+
 };
 
 
-typedef btScalar (*ContactSolverFunc)(btRigidBody& body1,
-									 btRigidBody& body2,
-									 class btManifoldPoint& contactPoint,
-									 const btContactSolverInfo& info);
-
-///stores some extra information to each contact point. It is not in the contact point, because that want to keep the collision detection independent from the constraint solver.
-struct btConstraintPersistentData
-{
-	inline btConstraintPersistentData()
-	:m_appliedImpulse(btScalar(0.)),
-	m_prevAppliedImpulse(btScalar(0.)),
-	m_accumulatedTangentImpulse0(btScalar(0.)),
-	m_accumulatedTangentImpulse1(btScalar(0.)),
-	m_jacDiagABInv(btScalar(0.)),
-	m_persistentLifeTime(0),
-	m_restitution(btScalar(0.)),
-	m_friction(btScalar(0.)),
-	m_penetration(btScalar(0.)),
-	m_contactSolverFunc(0),
-	m_frictionSolverFunc(0)
-	{
-	}
-	
-					
-				/// total applied impulse during most recent frame
-			btScalar	m_appliedImpulse;
-			btScalar	m_prevAppliedImpulse;
-			btScalar	m_accumulatedTangentImpulse0;
-			btScalar	m_accumulatedTangentImpulse1;
-			
-			btScalar	m_jacDiagABInv;
-			btScalar	m_jacDiagABInvTangent0;
-			btScalar	m_jacDiagABInvTangent1;
-			int		m_persistentLifeTime;
-			btScalar	m_restitution;
-			btScalar	m_friction;
-			btScalar	m_penetration;
-			btVector3	m_frictionWorldTangential0;
-			btVector3	m_frictionWorldTangential1;
-
-			btVector3	m_frictionAngularComponent0A;
-			btVector3	m_frictionAngularComponent0B;
-			btVector3	m_frictionAngularComponent1A;
-			btVector3	m_frictionAngularComponent1B;
-
-			//some data doesn't need to be persistent over frames: todo: clean/reuse this
-			btVector3	m_angularComponentA;
-			btVector3	m_angularComponentB;
-		
-			ContactSolverFunc	m_contactSolverFunc;
-			ContactSolverFunc	m_frictionSolverFunc;
-
-};
-
-///bilateral constraint between two dynamic objects
-///positive distance = separation, negative distance = penetration
+///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects
 void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
                       btRigidBody& body2, const btVector3& pos2,
@@ -95,28 +65,4 @@
 
 
-///contact constraint resolution:
-///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
-///positive distance = separation, negative distance = penetration
-btScalar resolveSingleCollision(
-	btRigidBody& body1,
-	btRigidBody& body2,
-		btManifoldPoint& contactPoint,
-		 const btContactSolverInfo& info);
-
-btScalar resolveSingleFriction(
-	btRigidBody& body1,
-	btRigidBody& body2,
-	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo
-		);
-
-
-
-btScalar resolveSingleCollisionCombined(
-	btRigidBody& body1,
-	btRigidBody& body2,
-	btManifoldPoint& contactPoint,
-	const btContactSolverInfo& solverInfo
-		);
 
 #endif //CONTACT_CONSTRAINT_H
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h	(revision 8284)
@@ -36,5 +36,5 @@
 
 	btScalar	m_tau;
-	btScalar	m_damping;
+	btScalar	m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
 	btScalar	m_friction;
 	btScalar	m_timeStep;
@@ -53,4 +53,5 @@
 	int			m_solverMode;
 	int	m_restingContactRestitutionThreshold;
+	int			m_minimumSolverBatchSize;
 
 
@@ -78,6 +79,7 @@
 		m_linearSlop = btScalar(0.0);
 		m_warmstartingFactor=btScalar(0.85);
-		m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD ;//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
 	}
 };
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp	(revision 8284)
@@ -23,18 +23,16 @@
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btTransformUtil.h"
 #include <new>
 
 
+
 #define D6_USE_OBSOLETE_METHOD false
-//-----------------------------------------------------------------------------
-
-btGeneric6DofConstraint::btGeneric6DofConstraint()
-:btTypedConstraint(D6_CONSTRAINT_TYPE),
-m_useLinearReferenceFrameA(true),
-m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
-{
-}
-
-//-----------------------------------------------------------------------------
+#define D6_USE_FRAME_OFFSET true
+
+
+
+
+
 
 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
@@ -43,14 +41,32 @@
 , m_frameInB(frameInB),
 m_useLinearReferenceFrameA(useLinearReferenceFrameA),
+m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
+m_flags(0),
 m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
 {
-
-}
-//-----------------------------------------------------------------------------
+	calculateTransforms();
+}
+
+
+
+btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
+        : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
+		m_frameInB(frameInB),
+		m_useLinearReferenceFrameA(useLinearReferenceFrameB),
+		m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
+		m_flags(0),
+		m_useSolveConstraintObsolete(false)
+{
+	///not providing rigidbody A means implicitly using worldspace for body A
+	m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
+	calculateTransforms();
+}
+
+
 
 
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
 
-//-----------------------------------------------------------------------------
+
 
 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
@@ -62,5 +78,5 @@
 }
 
-//-----------------------------------------------------------------------------
+
 
 ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
@@ -111,5 +127,4 @@
 		return 0;
 	}
-
 	if (test_value < m_loLimit)
 	{
@@ -130,9 +145,9 @@
 }
 
-//-----------------------------------------------------------------------------
+
 
 btScalar btRotationalLimitMotor::solveAngularLimits(
 	btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
-	btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
+	btRigidBody * body0, btRigidBody * body1 )
 {
 	if (needApplyTorques()==false) return 0.0f;
@@ -144,5 +159,5 @@
 	if (m_currentLimit!=0)
 	{
-		target_velocity = -m_ERP*m_currentLimitError/(timeStep);
+		target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
 		maxMotorForce = m_maxLimitForce;
 	}
@@ -153,7 +168,7 @@
 
 	btVector3 angVelA;
-	bodyA.getAngularVelocity(angVelA);
+	body0->internalGetAngularVelocity(angVelA);
 	btVector3 angVelB;
-	bodyB.getAngularVelocity(angVelB);
+	body1->internalGetAngularVelocity(angVelB);
 
 	btVector3 vel_diff;
@@ -192,6 +207,6 @@
 
 	// sort with accumulated impulses
-	btScalar	lo = btScalar(-1e30);
-	btScalar	hi = btScalar(1e30);
+	btScalar	lo = btScalar(-BT_LARGE_FLOAT);
+	btScalar	hi = btScalar(BT_LARGE_FLOAT);
 
 	btScalar oldaccumImpulse = m_accumulatedImpulse;
@@ -206,6 +221,6 @@
 	//body1->applyTorqueImpulse(-motorImp);
 
-	bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
-	bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
+	body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
+	body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
 
 
@@ -250,13 +265,13 @@
 	m_currentLimitError[limitIndex] = btScalar(0.f);
 	return 0;
-} // btTranslationalLimitMotor::testLimitValue()
-
-//-----------------------------------------------------------------------------
+}
+
+
 
 btScalar btTranslationalLimitMotor::solveLinearAxis(
 	btScalar timeStep,
 	btScalar jacDiagABInv,
-	btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
-	btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
+	btRigidBody& body1,const btVector3 &pointInA,
+	btRigidBody& body2,const btVector3 &pointInB,
 	int limit_index,
 	const btVector3 & axis_normal_on_a,
@@ -271,7 +286,7 @@
 
 	btVector3 vel1;
-	bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+	body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
 	btVector3 vel2;
-	bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+	body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
 	btVector3 vel = vel1 - vel2;
 
@@ -284,6 +299,6 @@
 	//positional error (zeroth order error)
 	btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
-	btScalar	lo = btScalar(-1e30);
-	btScalar	hi = btScalar(1e30);
+	btScalar	lo = btScalar(-BT_LARGE_FLOAT);
+	btScalar	hi = btScalar(BT_LARGE_FLOAT);
 
 	btScalar minLimit = m_lowerLimit[limit_index];
@@ -331,6 +346,6 @@
 	btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
 	btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
-	bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
-	bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
+	body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
+	body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
 
 
@@ -373,15 +388,34 @@
 }
 
-//-----------------------------------------------------------------------------
-
 void btGeneric6DofConstraint::calculateTransforms()
 {
-	m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
-	m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
+	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+
+void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+{
+	m_calculatedTransformA = transA * m_frameInA;
+	m_calculatedTransformB = transB * m_frameInB;
 	calculateLinearInfo();
 	calculateAngleInfo();
-}
-
-//-----------------------------------------------------------------------------
+	if(m_useOffsetForConstraintFrame)
+	{	//  get weight factors depending on masses
+		btScalar miA = getRigidBodyA().getInvMass();
+		btScalar miB = getRigidBodyB().getInvMass();
+		m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
+		btScalar miS = miA + miB;
+		if(miS > btScalar(0.f))
+		{
+			m_factA = miB / miS;
+		}
+		else 
+		{
+			m_factA = btScalar(0.5f);
+		}
+		m_factB = btScalar(1.0f) - m_factA;
+	}
+}
+
+
 
 void btGeneric6DofConstraint::buildLinearJacobian(
@@ -401,5 +435,5 @@
 }
 
-//-----------------------------------------------------------------------------
+
 
 void btGeneric6DofConstraint::buildAngularJacobian(
@@ -414,9 +448,11 @@
 }
 
-//-----------------------------------------------------------------------------
+
 
 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
 {
 	btScalar angle = m_calculatedAxisAngleDiff[axis_index];
+	angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
+	m_angularLimits[axis_index].m_currentPosition = angle;
 	//test limits
 	m_angularLimits[axis_index].testLimitValue(angle);
@@ -424,8 +460,9 @@
 }
 
-//-----------------------------------------------------------------------------
+
 
 void btGeneric6DofConstraint::buildJacobian()
 {
+#ifndef __SPU__
 	if (m_useSolveConstraintObsolete)
 	{
@@ -439,5 +476,5 @@
 		}
 		//calculates transform
-		calculateTransforms();
+		calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
 
 		//  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
@@ -482,7 +519,8 @@
 
 	}
-}
-
-//-----------------------------------------------------------------------------
+#endif //__SPU__
+
+}
+
 
 void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
@@ -495,5 +533,5 @@
 	{
 		//prepare constraint
-		calculateTransforms();
+		calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
 		info->m_numConstraintRows = 0;
 		info->nub = 6;
@@ -520,19 +558,74 @@
 }
 
-//-----------------------------------------------------------------------------
+void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
+	} else
+	{
+		//pre-allocate all 6
+		info->m_numConstraintRows = 6;
+		info->nub = 0;
+	}
+}
+
 
 void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
 {
 	btAssert(!m_useSolveConstraintObsolete);
-	int row = setLinearLimits(info);
-	setAngularLimits(info, row);
-}
-
-//-----------------------------------------------------------------------------
-
-int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
-{
-	btGeneric6DofConstraint * d6constraint = this;
-	int row = 0;
+
+	const btTransform& transA = m_rbA.getCenterOfMassTransform();
+	const btTransform& transB = m_rbB.getCenterOfMassTransform();
+	const btVector3& linVelA = m_rbA.getLinearVelocity();
+	const btVector3& linVelB = m_rbB.getLinearVelocity();
+	const btVector3& angVelA = m_rbA.getAngularVelocity();
+	const btVector3& angVelB = m_rbB.getAngularVelocity();
+
+	if(m_useOffsetForConstraintFrame)
+	{ // for stability better to solve angular limits first
+		int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
+		setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
+	}
+	else
+	{ // leave old version for compatibility
+		int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
+		setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
+	}
+
+}
+
+
+void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+{
+	
+	btAssert(!m_useSolveConstraintObsolete);
+	//prepare constraint
+	calculateTransforms(transA,transB);
+
+	int i;
+	for (i=0;i<3 ;i++ )
+	{
+		testAngularLimitMotor(i);
+	}
+
+	if(m_useOffsetForConstraintFrame)
+	{ // for stability better to solve angular limits first
+		int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
+		setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
+	}
+	else
+	{ // leave old version for compatibility
+		int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
+		setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
+	}
+}
+
+
+
+int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+{
+//	int row = 0;
 	//solve linear limits
 	btRotationalLimitMotor limot;
@@ -543,8 +636,8 @@
 			limot.m_bounce = btScalar(0.f);
 			limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
+			limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
 			limot.m_currentLimitError  = m_linearLimits.m_currentLimitError[i];
 			limot.m_damping  = m_linearLimits.m_damping;
 			limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
-			limot.m_ERP  = m_linearLimits.m_restitution;
 			limot.m_hiLimit  = m_linearLimits.m_upperLimit[i];
 			limot.m_limitSoftness  = m_linearLimits.m_limitSoftness;
@@ -554,5 +647,23 @@
 			limot.m_targetVelocity  = m_linearLimits.m_targetVelocity[i];
 			btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
-			row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0);
+			int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
+			limot.m_normalCFM	= (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
+			limot.m_stopCFM		= (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
+			limot.m_stopERP		= (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
+			if(m_useOffsetForConstraintFrame)
+			{
+				int indx1 = (i + 1) % 3;
+				int indx2 = (i + 2) % 3;
+				int rotAllowed = 1; // rotations around orthos to current axis
+				if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
+				{
+					rotAllowed = 0;
+				}
+				row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
+			}
+			else
+			{
+				row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
+			}
 		}
 	}
@@ -560,7 +671,7 @@
 }
 
-//-----------------------------------------------------------------------------
-
-int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset)
+
+
+int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
 {
 	btGeneric6DofConstraint * d6constraint = this;
@@ -572,9 +683,19 @@
 		{
 			btVector3 axis = d6constraint->getAxis(i);
-			row += get_limit_motor_info2(
-				d6constraint->getRotationalLimitMotor(i),
-				&m_rbA,
-				&m_rbB,
-				info,row,axis,1);
+			int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
+			if(!(flags & BT_6DOF_FLAGS_CFM_NORM))
+			{
+				m_angularLimits[i].m_normalCFM = info->cfm[0];
+			}
+			if(!(flags & BT_6DOF_FLAGS_CFM_STOP))
+			{
+				m_angularLimits[i].m_stopCFM = info->cfm[0];
+			}
+			if(!(flags & BT_6DOF_FLAGS_ERP_STOP))
+			{
+				m_angularLimits[i].m_stopERP = info->erp;
+			}
+			row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
+												transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
 		}
 	}
@@ -583,66 +704,6 @@
 }
 
-//-----------------------------------------------------------------------------
-
-void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
-{
-	if (m_useSolveConstraintObsolete)
-	{
-
-
-		m_timeStep = timeStep;
-
-		//calculateTransforms();
-
-		int i;
-
-		// linear
-
-		btVector3 pointInA = m_calculatedTransformA.getOrigin();
-		btVector3 pointInB = m_calculatedTransformB.getOrigin();
-
-		btScalar jacDiagABInv;
-		btVector3 linear_axis;
-		for (i=0;i<3;i++)
-		{
-			if (m_linearLimits.isLimited(i))
-			{
-				jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
-
-				if (m_useLinearReferenceFrameA)
-					linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
-				else
-					linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
-
-				m_linearLimits.solveLinearAxis(
-					m_timeStep,
-					jacDiagABInv,
-					m_rbA,bodyA,pointInA,
-					m_rbB,bodyB,pointInB,
-					i,linear_axis, m_AnchorPos);
-
-			}
-		}
-
-		// angular
-		btVector3 angular_axis;
-		btScalar angularJacDiagABInv;
-		for (i=0;i<3;i++)
-		{
-			if (m_angularLimits[i].needApplyTorques())
-			{
-
-				// get axis
-				angular_axis = getAxis(i);
-
-				angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
-
-				m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB);
-			}
-		}
-	}
-}
-
-//-----------------------------------------------------------------------------
+
+
 
 void	btGeneric6DofConstraint::updateRHS(btScalar	timeStep)
@@ -652,5 +713,5 @@
 }
 
-//-----------------------------------------------------------------------------
+
 
 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
@@ -659,12 +720,17 @@
 }
 
-//-----------------------------------------------------------------------------
-
-btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
-{
-	return m_calculatedAxisAngleDiff[axis_index];
-}
-
-//-----------------------------------------------------------------------------
+
+btScalar	btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
+{
+	return m_calculatedLinearDiff[axisIndex];
+}
+
+
+btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
+{
+	return m_calculatedAxisAngleDiff[axisIndex];
+}
+
+
 
 void btGeneric6DofConstraint::calcAnchorPos(void)
@@ -685,7 +751,7 @@
 	m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
 	return;
-} // btGeneric6DofConstraint::calcAnchorPos()
-
-//-----------------------------------------------------------------------------
+}
+
+
 
 void btGeneric6DofConstraint::calculateLinearInfo()
@@ -695,14 +761,15 @@
 	for(int i = 0; i < 3; i++)
 	{
+		m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
 		m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
 	}
-} // btGeneric6DofConstraint::calculateLinearInfo()
-
-//-----------------------------------------------------------------------------
+}
+
+
 
 int btGeneric6DofConstraint::get_limit_motor_info2(
 	btRotationalLimitMotor * limot,
-	btRigidBody * body0, btRigidBody * body1,
-	btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
+	const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
+	btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
 {
     int srow = row * info->rowskip;
@@ -722,18 +789,51 @@
             J2[srow+2] = -ax1[2];
         }
-        if((!rotational) && limit)
+        if((!rotational))
         {
-			btVector3 ltd;	// Linear Torque Decoupling vector
-			btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition();
-			ltd = c.cross(ax1);
-            info->m_J1angularAxis[srow+0] = ltd[0];
-            info->m_J1angularAxis[srow+1] = ltd[1];
-            info->m_J1angularAxis[srow+2] = ltd[2];
-
-			c = m_calculatedTransformB.getOrigin() - body1->getCenterOfMassPosition();
-			ltd = -c.cross(ax1);
-			info->m_J2angularAxis[srow+0] = ltd[0];
-            info->m_J2angularAxis[srow+1] = ltd[1];
-            info->m_J2angularAxis[srow+2] = ltd[2];
+			if (m_useOffsetForConstraintFrame)
+			{
+				btVector3 tmpA, tmpB, relA, relB;
+				// get vector from bodyB to frameB in WCS
+				relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
+				// get its projection to constraint axis
+				btVector3 projB = ax1 * relB.dot(ax1);
+				// get vector directed from bodyB to constraint axis (and orthogonal to it)
+				btVector3 orthoB = relB - projB;
+				// same for bodyA
+				relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
+				btVector3 projA = ax1 * relA.dot(ax1);
+				btVector3 orthoA = relA - projA;
+				// get desired offset between frames A and B along constraint axis
+				btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
+				// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
+				btVector3 totalDist = projA + ax1 * desiredOffs - projB;
+				// get offset vectors relA and relB
+				relA = orthoA + totalDist * m_factA;
+				relB = orthoB - totalDist * m_factB;
+				tmpA = relA.cross(ax1);
+				tmpB = relB.cross(ax1);
+				if(m_hasStaticBody && (!rotAllowed))
+				{
+					tmpA *= m_factA;
+					tmpB *= m_factB;
+				}
+				int i;
+				for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
+				for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
+			} else
+			{
+				btVector3 ltd;	// Linear Torque Decoupling vector
+				btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
+				ltd = c.cross(ax1);
+				info->m_J1angularAxis[srow+0] = ltd[0];
+				info->m_J1angularAxis[srow+1] = ltd[1];
+				info->m_J1angularAxis[srow+2] = ltd[2];
+
+				c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
+				ltd = -c.cross(ax1);
+				info->m_J2angularAxis[srow+0] = ltd[0];
+				info->m_J2angularAxis[srow+1] = ltd[1];
+				info->m_J2angularAxis[srow+2] = ltd[2];
+			}
         }
         // if we're limited low and high simultaneously, the joint motor is
@@ -743,8 +843,15 @@
         if (powered)
         {
-            info->cfm[srow] = 0.0f;
+			info->cfm[srow] = limot->m_normalCFM;
             if(!limit)
             {
-                info->m_constraintError[srow] += limot->m_targetVelocity;
+				btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
+
+				btScalar mot_fact = getMotorFactor(	limot->m_currentPosition, 
+													limot->m_loLimit,
+													limot->m_hiLimit, 
+													tag_vel, 
+													info->fps * limot->m_stopERP);
+				info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
                 info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
                 info->m_upperLimit[srow] = limot->m_maxMotorForce;
@@ -753,5 +860,5 @@
         if(limit)
         {
-            btScalar k = info->fps * limot->m_ERP;
+            btScalar k = info->fps * limot->m_stopERP;
 			if(!rotational)
 			{
@@ -762,5 +869,5 @@
 				info->m_constraintError[srow] += -k * limot->m_currentLimitError;
 			}
-            info->cfm[srow] = 0.0f;
+			info->cfm[srow] = limot->m_stopCFM;
             if (limot->m_loLimit == limot->m_hiLimit)
             {   // limited low and high simultaneously
@@ -787,13 +894,15 @@
                     if (rotational)
                     {
-                        vel = body0->getAngularVelocity().dot(ax1);
-                        if (body1)
-                            vel -= body1->getAngularVelocity().dot(ax1);
+                        vel = angVelA.dot(ax1);
+//make sure that if no body -> angVelB == zero vec
+//                        if (body1)
+                            vel -= angVelB.dot(ax1);
                     }
                     else
                     {
-                        vel = body0->getLinearVelocity().dot(ax1);
-                        if (body1)
-                            vel -= body1->getLinearVelocity().dot(ax1);
+                        vel = linVelA.dot(ax1);
+//make sure that if no body -> angVelB == zero vec
+//                        if (body1)
+                            vel -= linVelB.dot(ax1);
                     }
                     // only apply bounce if the velocity is incoming, and if the
@@ -825,5 +934,107 @@
 }
 
-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
+
+
+
+
+
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///If no axis is provided, it uses the default axis for this constraint.
+void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
+{
+	if((axis >= 0) && (axis < 3))
+	{
+		switch(num)
+		{
+			case BT_CONSTRAINT_STOP_ERP : 
+				m_linearLimits.m_stopERP[axis] = value;
+				m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+				break;
+			case BT_CONSTRAINT_STOP_CFM : 
+				m_linearLimits.m_stopCFM[axis] = value;
+				m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+				break;
+			case BT_CONSTRAINT_CFM : 
+				m_linearLimits.m_normalCFM[axis] = value;
+				m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+				break;
+			default : 
+				btAssertConstrParams(0);
+		}
+	}
+	else if((axis >=3) && (axis < 6))
+	{
+		switch(num)
+		{
+			case BT_CONSTRAINT_STOP_ERP : 
+				m_angularLimits[axis - 3].m_stopERP = value;
+				m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+				break;
+			case BT_CONSTRAINT_STOP_CFM : 
+				m_angularLimits[axis - 3].m_stopCFM = value;
+				m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+				break;
+			case BT_CONSTRAINT_CFM : 
+				m_angularLimits[axis - 3].m_normalCFM = value;
+				m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
+				break;
+			default : 
+				btAssertConstrParams(0);
+		}
+	}
+	else
+	{
+		btAssertConstrParams(0);
+	}
+}
+
+	///return the local value of parameter
+btScalar btGeneric6DofConstraint::getParam(int num, int axis) const 
+{
+	btScalar retVal = 0;
+	if((axis >= 0) && (axis < 3))
+	{
+		switch(num)
+		{
+			case BT_CONSTRAINT_STOP_ERP : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+				retVal = m_linearLimits.m_stopERP[axis];
+				break;
+			case BT_CONSTRAINT_STOP_CFM : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+				retVal = m_linearLimits.m_stopCFM[axis];
+				break;
+			case BT_CONSTRAINT_CFM : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+				retVal = m_linearLimits.m_normalCFM[axis];
+				break;
+			default : 
+				btAssertConstrParams(0);
+		}
+	}
+	else if((axis >=3) && (axis < 6))
+	{
+		switch(num)
+		{
+			case BT_CONSTRAINT_STOP_ERP : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+				retVal = m_angularLimits[axis - 3].m_stopERP;
+				break;
+			case BT_CONSTRAINT_STOP_CFM : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+				retVal = m_angularLimits[axis - 3].m_stopCFM;
+				break;
+			case BT_CONSTRAINT_CFM : 
+				btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
+				retVal = m_angularLimits[axis - 3].m_normalCFM;
+				break;
+			default : 
+				btAssertConstrParams(0);
+		}
+	}
+	else
+	{
+		btAssertConstrParams(0);
+	}
+	return retVal;
+}
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h	(revision 8284)
@@ -13,4 +13,8 @@
 3. This notice may not be removed or altered from any source distribution.
 */
+
+/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev
+/// Added support for generic constraint solver through getInfo1/getInfo2 methods
+
 /*
 2007-09-09
@@ -46,5 +50,7 @@
     btScalar m_damping;//!< Damping.
     btScalar m_limitSoftness;//! Relaxation factor
-    btScalar m_ERP;//!< Error tolerance factor when joint is at limit
+    btScalar m_normalCFM;//!< Constraint force mixing factor
+    btScalar m_stopERP;//!< Error tolerance factor when joint is at limit
+    btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
     btScalar m_bounce;//!< restitution factor
     bool m_enableMotor;
@@ -55,4 +61,5 @@
     //!@{
     btScalar m_currentLimitError;//!  How much is violated this limit
+    btScalar m_currentPosition;     //!  current value of angle 
     int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
     btScalar m_accumulatedImpulse;
@@ -65,7 +72,9 @@
         m_maxMotorForce = 0.1f;
         m_maxLimitForce = 300.0f;
-        m_loLimit = -SIMD_INFINITY;
-        m_hiLimit = SIMD_INFINITY;
-        m_ERP = 0.5f;
+        m_loLimit = 1.0f;
+        m_hiLimit = -1.0f;
+		m_normalCFM = 0.f;
+		m_stopERP = 0.2f;
+		m_stopCFM = 0.f;
         m_bounce = 0.0f;
         m_damping = 1.0f;
@@ -83,5 +92,7 @@
         m_loLimit = limot.m_loLimit;
         m_hiLimit = limot.m_hiLimit;
-        m_ERP = limot.m_ERP;
+		m_normalCFM = limot.m_normalCFM;
+		m_stopERP = limot.m_stopERP;
+		m_stopCFM =	limot.m_stopCFM;
         m_bounce = limot.m_bounce;
         m_currentLimit = limot.m_currentLimit;
@@ -113,5 +124,5 @@
 
 	//! apply the correction impulses for two bodies
-    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
+    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
 
 };
@@ -130,4 +141,7 @@
     btScalar	m_damping;//!< Damping for linear limit
     btScalar	m_restitution;//! Bounce parameter for linear limit
+	btVector3	m_normalCFM;//!< Constraint force mixing factor
+    btVector3	m_stopERP;//!< Error tolerance factor when joint is at limit
+	btVector3	m_stopCFM;//!< Constraint force mixing factor when joint is at limit
     //!@}
 	bool		m_enableMotor[3];
@@ -135,4 +149,5 @@
     btVector3	m_maxMotorForce;//!< max force on motor
     btVector3	m_currentLimitError;//!  How much is violated this limit
+    btVector3	m_currentLinearDiff;//!  Current relative offset of constraint frames
     int			m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
 
@@ -142,4 +157,7 @@
     	m_upperLimit.setValue(0.f,0.f,0.f);
     	m_accumulatedImpulse.setValue(0.f,0.f,0.f);
+		m_normalCFM.setValue(0.f, 0.f, 0.f);
+		m_stopERP.setValue(0.2f, 0.2f, 0.2f);
+		m_stopCFM.setValue(0.f, 0.f, 0.f);
 
     	m_limitSoftness = 0.7f;
@@ -163,4 +181,8 @@
     	m_damping = other.m_damping;
     	m_restitution = other.m_restitution;
+		m_normalCFM = other.m_normalCFM;
+		m_stopERP = other.m_stopERP;
+		m_stopCFM = other.m_stopCFM;
+
 		for(int i=0; i < 3; i++) 
 		{
@@ -193,6 +215,6 @@
     	btScalar timeStep,
         btScalar jacDiagABInv,
-        btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
-        btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
+        btRigidBody& body1,const btVector3 &pointInA,
+        btRigidBody& body2,const btVector3 &pointInB,
         int limit_index,
         const btVector3 & axis_normal_on_a,
@@ -201,4 +223,13 @@
 
 };
+
+enum bt6DofFlags
+{
+	BT_6DOF_FLAGS_CFM_NORM = 1,
+	BT_6DOF_FLAGS_CFM_STOP = 2,
+	BT_6DOF_FLAGS_ERP_STOP = 4
+};
+#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
+
 
 /// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
@@ -216,18 +247,20 @@
 <li> Angulars limits have these possible ranges:
 <table border=1 >
-<tr
-
+<tr>
 	<td><b>AXIS</b></td>
 	<td><b>MIN ANGLE</b></td>
 	<td><b>MAX ANGLE</b></td>
+</tr><tr>
 	<td>X</td>
-		<td>-PI</td>
-		<td>PI</td>
+	<td>-PI</td>
+	<td>PI</td>
+</tr><tr>
 	<td>Y</td>
-		<td>-PI/2</td>
-		<td>PI/2</td>
+	<td>-PI/2</td>
+	<td>PI/2</td>
+</tr><tr>
 	<td>Z</td>
-		<td>-PI/2</td>
-		<td>PI/2</td>
+	<td>-PI</td>
+	<td>PI</td>
 </tr>
 </table>
@@ -273,9 +306,15 @@
     btVector3 m_calculatedAxis[3];
     btVector3 m_calculatedLinearDiff;
+	btScalar	m_factA;
+	btScalar	m_factB;
+	bool		m_hasStaticBody;
     
 	btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
 
     bool	m_useLinearReferenceFrameA;
+	bool	m_useOffsetForConstraintFrame;
     
+	int		m_flags;
+
     //!@}
 
@@ -288,7 +327,7 @@
 
 
-	int setAngularLimits(btConstraintInfo2 *info, int row_offset);
-
-	int setLinearLimits(btConstraintInfo2 *info);
+	int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+
+	int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
 
     void buildLinearJacobian(
@@ -312,7 +351,6 @@
 
     btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
-
-    btGeneric6DofConstraint();
-
+    btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
+    
 	//! Calcs global transform of the offsets
 	/*!
@@ -320,5 +358,7 @@
 	\sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
 	*/
-    void calculateTransforms();
+    void calculateTransforms(const btTransform& transA,const btTransform& transB);
+
+	void calculateTransforms();
 
 	//! Gets the global transform of the offset for body A
@@ -367,7 +407,10 @@
 	virtual void getInfo1 (btConstraintInfo1* info);
 
+	void getInfo1NonVirtual (btConstraintInfo1* info);
+
 	virtual void getInfo2 (btConstraintInfo2* info);
 
-    virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
+	void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
+
 
     void	updateRHS(btScalar	timeStep);
@@ -381,12 +424,19 @@
     //! Get the relative Euler angle
     /*!
-	\pre btGeneric6DofConstraint.buildJacobian must be called previously.
+	\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
 	*/
     btScalar getAngle(int axis_index) const;
+
+	//! Get the relative position of the constraint pivot
+    /*!
+	\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
+	*/
+	btScalar getRelativePivotPosition(int axis_index) const;
+
 
 	//! Test angular limit.
 	/*!
 	Calculates angular correction and returns true if limit needs to be corrected.
-	\pre btGeneric6DofConstraint.buildJacobian must be called previously.
+	\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
 	*/
     bool testAngularLimitMotor(int axis_index);
@@ -404,14 +454,12 @@
     void	setAngularLowerLimit(const btVector3& angularLower)
     {
-        m_angularLimits[0].m_loLimit = angularLower.getX();
-        m_angularLimits[1].m_loLimit = angularLower.getY();
-        m_angularLimits[2].m_loLimit = angularLower.getZ();
+		for(int i = 0; i < 3; i++) 
+			m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
     }
 
     void	setAngularUpperLimit(const btVector3& angularUpper)
     {
-        m_angularLimits[0].m_hiLimit = angularUpper.getX();
-        m_angularLimits[1].m_hiLimit = angularUpper.getY();
-        m_angularLimits[2].m_hiLimit = angularUpper.getZ();
+		for(int i = 0; i < 3; i++)
+			m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
     }
 
@@ -438,4 +486,6 @@
     	else
     	{
+			lo = btNormalizeAngle(lo);
+			hi = btNormalizeAngle(hi);
     		m_angularLimits[axis-3].m_loLimit = lo;
     		m_angularLimits[axis-3].m_hiLimit = hi;
@@ -460,21 +510,79 @@
     }
 
-    const btRigidBody& getRigidBodyA() const
-    {
-        return m_rbA;
-    }
-    const btRigidBody& getRigidBodyB() const
-    {
-        return m_rbB;
-    }
-
 	virtual void calcAnchorPos(void); // overridable
 
 	int get_limit_motor_info2(	btRotationalLimitMotor * limot,
-								btRigidBody * body0, btRigidBody * body1,
-								btConstraintInfo2 *info, int row, btVector3& ax1, int rotational);
-
-
+								const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
+								btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
+
+	// access for UseFrameOffset
+	bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
+	void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
+
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///If no axis is provided, it uses the default axis for this constraint.
+	virtual	void setParam(int num, btScalar value, int axis = -1);
+	///return the local value of parameter
+	virtual	btScalar getParam(int num, int axis = -1) const;
+
+	virtual	int	calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+
+	
 };
 
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btGeneric6DofConstraintData
+{
+	btTypedConstraintData	m_typeConstraintData;
+	btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTransformFloatData m_rbBFrame;
+	
+	btVector3FloatData	m_linearUpperLimit;
+	btVector3FloatData	m_linearLowerLimit;
+
+	btVector3FloatData	m_angularUpperLimit;
+	btVector3FloatData	m_angularLowerLimit;
+	
+	int	m_useLinearReferenceFrameA;
+	int m_useOffsetForConstraintFrame;
+};
+
+SIMD_FORCE_INLINE	int	btGeneric6DofConstraint::calculateSerializeBufferSize() const
+{
+	return sizeof(btGeneric6DofConstraintData);
+}
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE	const char*	btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+
+	btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer;
+	btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
+
+	m_frameInA.serializeFloat(dof->m_rbAFrame);
+	m_frameInB.serializeFloat(dof->m_rbBFrame);
+
+		
+	int i;
+	for (i=0;i<3;i++)
+	{
+		dof->m_angularLowerLimit.m_floats[i] =  float(m_angularLimits[i].m_loLimit);
+		dof->m_angularUpperLimit.m_floats[i] =  float(m_angularLimits[i].m_hiLimit);
+		dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]);
+		dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]);
+	}
+	
+	dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
+	dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
+
+	return "btGeneric6DofConstraintData";
+}
+
+
+
+
+
 #endif //GENERIC_6DOF_CONSTRAINT_H
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp	(revision 8284)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp	(revision 8284)
@@ -0,0 +1,151 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. 
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btGeneric6DofSpringConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+
+
+btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA)
+	: btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
+{
+	for(int i = 0; i < 6; i++)
+	{
+		m_springEnabled[i] = false;
+		m_equilibriumPoint[i] = btScalar(0.f);
+		m_springStiffness[i] = btScalar(0.f);
+		m_springDamping[i] = btScalar(1.f);
+	}
+}
+
+
+void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
+{
+	btAssert((index >= 0) && (index < 6));
+	m_springEnabled[index] = onOff;
+	if(index < 3)
+	{
+		m_linearLimits.m_enableMotor[index] = onOff;
+	}
+	else
+	{
+		m_angularLimits[index - 3].m_enableMotor = onOff;
+	}
+}
+
+
+
+void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness)
+{
+	btAssert((index >= 0) && (index < 6));
+	m_springStiffness[index] = stiffness;
+}
+
+
+void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping)
+{
+	btAssert((index >= 0) && (index < 6));
+	m_springDamping[index] = damping;
+}
+
+
+void btGeneric6DofSpringConstraint::setEquilibriumPoint()
+{
+	calculateTransforms();
+	int i;
+
+	for( i = 0; i < 3; i++)
+	{
+		m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
+	}
+	for(i = 0; i < 3; i++)
+	{
+		m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i];
+	}
+}
+
+
+
+void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
+{
+	btAssert((index >= 0) && (index < 6));
+	calculateTransforms();
+	if(index < 3)
+	{
+		m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
+	}
+	else
+	{
+		m_equilibriumPoint[index] = m_calculatedAxisAngleDiff[index - 3];
+	}
+}
+
+void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index, btScalar val)
+{
+	btAssert((index >= 0) && (index < 6));
+	m_equilibriumPoint[index] = val;
+}
+
+
+void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
+{
+	// it is assumed that calculateTransforms() have been called before this call
+	int i;
+	btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
+	for(i = 0; i < 3; i++)
+	{
+		if(m_springEnabled[i])
+		{
+			// get current position of constraint
+			btScalar currPos = m_calculatedLinearDiff[i];
+			// calculate difference
+			btScalar delta = currPos - m_equilibriumPoint[i];
+			// spring force is (delta * m_stiffness) according to Hooke's Law
+			btScalar force = delta * m_springStiffness[i];
+			btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations);
+			m_linearLimits.m_targetVelocity[i] =  velFactor * force;
+			m_linearLimits.m_maxMotorForce[i] =  btFabs(force) / info->fps;
+		}
+	}
+	for(i = 0; i < 3; i++)
+	{
+		if(m_springEnabled[i + 3])
+		{
+			// get current position of constraint
+			btScalar currPos = m_calculatedAxisAngleDiff[i];
+			// calculate difference
+			btScalar delta = currPos - m_equilibriumPoint[i+3];
+			// spring force is (-delta * m_stiffness) according to Hooke's Law
+			btScalar force = -delta * m_springStiffness[i+3];
+			btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations);
+			m_angularLimits[i].m_targetVelocity = velFactor * force;
+			m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
+		}
+	}
+}
+
+
+void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
+{
+	// this will be called by constraint solver at the constraint setup stage
+	// set current motor parameters
+	internalUpdateSprings(info);
+	// do the rest of job for constraint setup
+	btGeneric6DofConstraint::getInfo2(info);
+}
+
+
+
+
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h	(revision 8284)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h	(revision 8284)
@@ -0,0 +1,55 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. 
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef GENERIC_6DOF_SPRING_CONSTRAINT_H
+#define GENERIC_6DOF_SPRING_CONSTRAINT_H
+
+
+#include "LinearMath/btVector3.h"
+#include "btTypedConstraint.h"
+#include "btGeneric6DofConstraint.h"
+
+
+/// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF
+
+/// DOF index used in enableSpring() and setStiffness() means:
+/// 0 : translation X
+/// 1 : translation Y
+/// 2 : translation Z
+/// 3 : rotation X (3rd Euler rotational around new position of X axis, range [-PI+epsilon, PI-epsilon] )
+/// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] )
+/// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] )
+
+class btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
+{
+protected:
+	bool		m_springEnabled[6];
+	btScalar	m_equilibriumPoint[6];
+	btScalar	m_springStiffness[6];
+	btScalar	m_springDamping[6]; // between 0 and 1 (1 == no damping)
+	void internalUpdateSprings(btConstraintInfo2* info);
+public: 
+    btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
+	void enableSpring(int index, bool onOff);
+	void setStiffness(int index, btScalar stiffness);
+	void setDamping(int index, btScalar damping);
+	void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
+	void setEquilibriumPoint(int index);  // set the current constraint position/orientation as an equilibrium point for given DOF
+	void setEquilibriumPoint(int index, btScalar val);
+	virtual void getInfo2 (btConstraintInfo2* info);
+};
+
+#endif // GENERIC_6DOF_SPRING_CONSTRAINT_H
+
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp	(revision 8284)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp	(revision 8284)
@@ -0,0 +1,66 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. 
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "btHinge2Constraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+
+
+
+// constructor
+// anchor, axis1 and axis2 are in world coordinate system
+// axis1 must be orthogonal to axis2
+btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
+: btGeneric6DofSpringConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
+ m_anchor(anchor),
+ m_axis1(axis1),
+ m_axis2(axis2)
+{
+	// build frame basis
+	// 6DOF constraint uses Euler angles and to define limits
+	// it is assumed that rotational order is :
+	// Z - first, allowed limits are (-PI,PI);
+	// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number 
+	// used to prevent constraint from instability on poles;
+	// new position of X, allowed limits are (-PI,PI);
+	// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
+	// Build the frame in world coordinate system first
+	btVector3 zAxis = axis1.normalize();
+	btVector3 xAxis = axis2.normalize();
+	btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system
+	btTransform frameInW;
+	frameInW.setIdentity();
+	frameInW.getBasis().setValue(	xAxis[0], yAxis[0], zAxis[0],	
+									xAxis[1], yAxis[1], zAxis[1],
+									xAxis[2], yAxis[2], zAxis[2]);
+	frameInW.setOrigin(anchor);
+	// now get constraint frame in local coordinate systems
+	m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
+	m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
+	// sei limits
+	setLinearLowerLimit(btVector3(0.f, 0.f, -1.f));
+	setLinearUpperLimit(btVector3(0.f, 0.f,  1.f));
+	// like front wheels of a car
+	setAngularLowerLimit(btVector3(1.f,  0.f, -SIMD_HALF_PI * 0.5f)); 
+	setAngularUpperLimit(btVector3(-1.f, 0.f,  SIMD_HALF_PI * 0.5f));
+	// enable suspension
+	enableSpring(2, true);
+	setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-)
+	setDamping(2, 0.01f);
+	setEquilibriumPoint();
+}
+
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHinge2Constraint.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHinge2Constraint.h	(revision 8284)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHinge2Constraint.h	(revision 8284)
@@ -0,0 +1,58 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. 
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef HINGE2_CONSTRAINT_H
+#define HINGE2_CONSTRAINT_H
+
+
+
+#include "LinearMath/btVector3.h"
+#include "btTypedConstraint.h"
+#include "btGeneric6DofSpringConstraint.h"
+
+
+
+// Constraint similar to ODE Hinge2 Joint
+// has 3 degrees of frredom:
+// 2 rotational degrees of freedom, similar to Euler rotations around Z (axis 1) and X (axis 2)
+// 1 translational (along axis Z) with suspension spring
+
+class btHinge2Constraint : public btGeneric6DofSpringConstraint
+{
+protected:
+	btVector3	m_anchor;
+	btVector3	m_axis1;
+	btVector3	m_axis2;
+public:
+	// constructor
+	// anchor, axis1 and axis2 are in world coordinate system
+	// axis1 must be orthogonal to axis2
+    btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2);
+	// access
+	const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
+	const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
+	const btVector3& getAxis1() { return m_axis1; }
+	const btVector3& getAxis2() { return m_axis2; }
+	btScalar getAngle1() { return getAngle(2); }
+	btScalar getAngle2() { return getAngle(0); }
+	// limits
+	void setUpperLimit(btScalar ang1max) { setAngularUpperLimit(btVector3(-1.f, 0.f, ang1max)); }
+	void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3( 1.f, 0.f, ang1min)); }
+};
+
+
+
+#endif // HINGE2_CONSTRAINT_H
+
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp	(revision 8284)
@@ -22,29 +22,26 @@
 #include "btSolverBody.h"
 
-//-----------------------------------------------------------------------------
-
+
+
+//#define HINGE_USE_OBSOLETE_SOLVER false
 #define HINGE_USE_OBSOLETE_SOLVER false
 
-//-----------------------------------------------------------------------------
-
-
-btHingeConstraint::btHingeConstraint()
-: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
-m_enableAngularMotor(false),
-m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
-m_useReferenceFrameA(false)
-{
-	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
-}
-
-//-----------------------------------------------------------------------------
+#define HINGE_USE_FRAME_OFFSET true
+
+#ifndef __SPU__
+
+
+
+
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
-									 btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
+									 const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
 									 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
 									 m_angularOnly(false),
 									 m_enableAngularMotor(false),
 									 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
-									 m_useReferenceFrameA(useReferenceFrameA)
+									 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+									 m_useReferenceFrameA(useReferenceFrameA),
+									 m_flags(0)
 {
 	m_rbAFrame.getOrigin() = pivotInA;
@@ -80,6 +77,6 @@
 	
 	//start with free
-	m_lowerLimit = btScalar(1e30);
-	m_upperLimit = btScalar(-1e30);
+	m_lowerLimit = btScalar(1.0f);
+	m_upperLimit = btScalar(-1.0f);
 	m_biasFactor = 0.3f;
 	m_relaxationFactor = 1.0f;
@@ -89,10 +86,12 @@
 }
 
-//-----------------------------------------------------------------------------
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA)
+
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), 
 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
-m_useReferenceFrameA(useReferenceFrameA)
+m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+m_useReferenceFrameA(useReferenceFrameA),
+m_flags(0)
 {
 
@@ -120,6 +119,6 @@
 	
 	//start with free
-	m_lowerLimit = btScalar(1e30);
-	m_upperLimit = btScalar(-1e30);
+	m_lowerLimit = btScalar(1.0f);
+	m_upperLimit = btScalar(-1.0f);
 	m_biasFactor = 0.3f;
 	m_relaxationFactor = 1.0f;
@@ -129,5 +128,5 @@
 }
 
-//-----------------------------------------------------------------------------
+
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
@@ -137,9 +136,11 @@
 m_enableAngularMotor(false),
 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
-m_useReferenceFrameA(useReferenceFrameA)
+m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+m_useReferenceFrameA(useReferenceFrameA),
+m_flags(0)
 {
 	//start with free
-	m_lowerLimit = btScalar(1e30);
-	m_upperLimit = btScalar(-1e30);
+	m_lowerLimit = btScalar(1.0f);
+	m_upperLimit = btScalar(-1.0f);
 	m_biasFactor = 0.3f;
 	m_relaxationFactor = 1.0f;
@@ -149,5 +150,5 @@
 }			
 
-//-----------------------------------------------------------------------------
+
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
@@ -156,5 +157,7 @@
 m_enableAngularMotor(false),
 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
-m_useReferenceFrameA(useReferenceFrameA)
+m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+m_useReferenceFrameA(useReferenceFrameA),
+m_flags(0)
 {
 	///not providing rigidbody B means implicitly using worldspace for body B
@@ -163,6 +166,6 @@
 
 	//start with free
-	m_lowerLimit = btScalar(1e30);
-	m_upperLimit = btScalar(-1e30);	
+	m_lowerLimit = btScalar(1.0f);
+	m_upperLimit = btScalar(-1.0f);
 	m_biasFactor = 0.3f;
 	m_relaxationFactor = 1.0f;
@@ -172,5 +175,5 @@
 }
 
-//-----------------------------------------------------------------------------
+
 
 void	btHingeConstraint::buildJacobian()
@@ -179,4 +182,5 @@
 	{
 		m_appliedImpulse = btScalar(0.);
+		m_accMotorImpulse = btScalar(0.);
 
 		if (!m_angularOnly)
@@ -222,5 +226,4 @@
 		btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
 
-		getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
 		btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
 		btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
@@ -249,5 +252,5 @@
 
 			// test angular limit
-			testLimit();
+			testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
 
 		//Compute K = J*W*J' for hinge axis
@@ -259,5 +262,7 @@
 }
 
-//-----------------------------------------------------------------------------
+
+#endif //__SPU__
+
 
 void btHingeConstraint::getInfo1(btConstraintInfo1* info)
@@ -272,6 +277,7 @@
 		info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
 		info->nub = 1; 
+		//always add the row, to avoid computation (data is not available yet)
 		//prepare constraint
-		testLimit();
+		testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
 		if(getSolveLimit() || getEnableAngularMotor())
 		{
@@ -279,44 +285,113 @@
 			info->nub--; 
 		}
-	}
-} // btHingeConstraint::getInfo1 ()
-
-//-----------------------------------------------------------------------------
+
+	}
+}
+
+void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
+	}
+	else
+	{
+		//always add the 'limit' row, to avoid computation (data is not available yet)
+		info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular
+		info->nub = 0; 
+	}
+}
 
 void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
 {
+	if(m_useOffsetForConstraintFrame)
+	{
+		getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
+	}
+	else
+	{
+		getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
+	}
+}
+
+
+void	btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+{
+	///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now
+	testLimit(transA,transB);
+
+	getInfo2Internal(info,transA,transB,angVelA,angVelB);
+}
+
+
+void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+{
+
 	btAssert(!m_useSolveConstraintObsolete);
-	int i, s = info->rowskip;
+	int i, skip = info->rowskip;
 	// transforms in world space
-	btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
-	btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
+	btTransform trA = transA*m_rbAFrame;
+	btTransform trB = transB*m_rbBFrame;
 	// pivot point
 	btVector3 pivotAInW = trA.getOrigin();
 	btVector3 pivotBInW = trB.getOrigin();
+#if 0
+	if (0)
+	{
+		for (i=0;i<6;i++)
+		{
+			info->m_J1linearAxis[i*skip]=0;
+			info->m_J1linearAxis[i*skip+1]=0;
+			info->m_J1linearAxis[i*skip+2]=0;
+
+			info->m_J1angularAxis[i*skip]=0;
+			info->m_J1angularAxis[i*skip+1]=0;
+			info->m_J1angularAxis[i*skip+2]=0;
+
+			info->m_J2angularAxis[i*skip]=0;
+			info->m_J2angularAxis[i*skip+1]=0;
+			info->m_J2angularAxis[i*skip+2]=0;
+
+			info->m_constraintError[i*skip]=0.f;
+		}
+	}
+#endif //#if 0
 	// linear (all fixed)
-    info->m_J1linearAxis[0] = 1;
-    info->m_J1linearAxis[s + 1] = 1;
-    info->m_J1linearAxis[2 * s + 2] = 1;
-	btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin();
+
+	if (!m_angularOnly)
+	{
+		info->m_J1linearAxis[0] = 1;
+		info->m_J1linearAxis[skip + 1] = 1;
+		info->m_J1linearAxis[2 * skip + 2] = 1;
+	}	
+
+
+
+
+	btVector3 a1 = pivotAInW - transA.getOrigin();
 	{
 		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
-		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s);
+		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
 		btVector3 a1neg = -a1;
 		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
 	}
-	btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
+	btVector3 a2 = pivotBInW - transB.getOrigin();
 	{
 		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
-		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
 		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
 	}
 	// linear RHS
     btScalar k = info->fps * info->erp;
-	for(i = 0; i < 3; i++)
-    {
-        info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]);
-    }
+	if (!m_angularOnly)
+	{
+		for(i = 0; i < 3; i++)
+		{
+			info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
+		}
+	}
 	// make rotations around X and Y equal
 	// the hinge axis should be the only unconstrained
@@ -403,8 +478,12 @@
 		}
 		info->m_constraintError[srow] = btScalar(0.0f);
+		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
 		if(powered)
 		{
-            info->cfm[srow] = btScalar(0.0); 
-			btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
+			if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
+			{
+				info->cfm[srow] = m_normalCFM;
+			}
+			btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
 			info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
 			info->m_lowerLimit[srow] = - m_maxMotorImpulse;
@@ -413,7 +492,10 @@
 		if(limit)
 		{
-			k = info->fps * info->erp;
+			k = info->fps * currERP;
 			info->m_constraintError[srow] += k * limit_err;
-			info->cfm[srow] = btScalar(0.0);
+			if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
+			{
+				info->cfm[srow] = m_stopCFM;
+			}
 			if(lostop == histop) 
 			{
@@ -436,6 +518,6 @@
 			if(bounce > btScalar(0.0))
 			{
-				btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
-				vel -= m_rbB.getAngularVelocity().dot(ax1);
+				btScalar vel = angVelA.dot(ax1);
+				vel -= angVelB.dot(ax1);
 				// only apply bounce if the velocity is incoming, and if the
 				// resulting c[] exceeds what we already have.
@@ -468,153 +550,8 @@
 }
 
-//-----------------------------------------------------------------------------
-
-void	btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
-{
-
-	///for backwards compatibility during the transition to 'getInfo/getInfo2'
-	if (m_useSolveConstraintObsolete)
-	{
-
-		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
-
-		btScalar tau = btScalar(0.3);
-
-		//linear part
-		if (!m_angularOnly)
-		{
-			btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
-			btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-
-			btVector3 vel1,vel2;
-			bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
-			bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
-			btVector3 vel = vel1 - vel2;
-
-			for (int i=0;i<3;i++)
-			{		
-				const btVector3& normal = m_jac[i].m_linearJointAxis;
-				btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
-
-				btScalar rel_vel;
-				rel_vel = normal.dot(vel);
-				//positional error (zeroth order error)
-				btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
-				btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
-				m_appliedImpulse += impulse;
-				btVector3 impulse_vector = normal * impulse;
-				btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
-				btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
-				bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
-				bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
-			}
-		}
-
-		
-		{
-			///solve angular part
-
-			// get axes in world space
-			btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
-			btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
-
-			btVector3 angVelA;
-			bodyA.getAngularVelocity(angVelA);
-			btVector3 angVelB;
-			bodyB.getAngularVelocity(angVelB);
-
-			btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
-			btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
-
-			btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
-			btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
-			btVector3 velrelOrthog = angAorthog-angBorthog;
-			{
-				
-
-				//solve orthogonal angular velocity correction
-				btScalar relaxation = btScalar(1.);
-				btScalar len = velrelOrthog.length();
-				if (len > btScalar(0.00001))
-				{
-					btVector3 normal = velrelOrthog.normalized();
-					btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
-						getRigidBodyB().computeAngularImpulseDenominator(normal);
-					// scale for mass and relaxation
-					//velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
-
-					bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom));
-					bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom));
-
-				}
-
-				//solve angular positional correction
-				btVector3 angularError =  axisA.cross(axisB) *(btScalar(1.)/timeStep);
-				btScalar len2 = angularError.length();
-				if (len2>btScalar(0.00001))
-				{
-					btVector3 normal2 = angularError.normalized();
-					btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
-							getRigidBodyB().computeAngularImpulseDenominator(normal2);
-					//angularError *= (btScalar(1.)/denom2) * relaxation;
-					
-					bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2));
-					bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2));
-
-				}
-				
-				
-
-
-
-				// solve limit
-				if (m_solveLimit)
-				{
-					btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
-
-					btScalar impulseMag = amplitude * m_kHinge;
-
-					// Clamp the accumulated impulse
-					btScalar temp = m_accLimitImpulse;
-					m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
-					impulseMag = m_accLimitImpulse - temp;
-
-
-					
-					bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign);
-					bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign));
-
-				}
-			}
-
-			//apply motor
-			if (m_enableAngularMotor) 
-			{
-				//todo: add limits too
-				btVector3 angularLimit(0,0,0);
-
-				btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
-				btScalar projRelVel = velrel.dot(axisA);
-
-				btScalar desiredMotorVel = m_motorTargetVelocity;
-				btScalar motor_relvel = desiredMotorVel - projRelVel;
-
-				btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
-				//todo: should clip against accumulated impulse
-				btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
-				clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
-				btVector3 motorImp = clippedMotorImpulse * axisA;
-			
-				bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
-				bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
-				
-			}
-		}
-	}
-
-}
-
-//-----------------------------------------------------------------------------
+
+
+
+
 
 void	btHingeConstraint::updateRHS(btScalar	timeStep)
@@ -624,17 +561,22 @@
 }
 
-//-----------------------------------------------------------------------------
 
 btScalar btHingeConstraint::getHingeAngle()
 {
-	const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
-	const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
-	const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
-	btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+	return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+
+btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
+{
+	const btVector3 refAxis0  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
+	const btVector3 refAxis1  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
+	const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
+//	btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+	btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
 	return m_referenceSign * angle;
 }
 
-//-----------------------------------------------------------------------------
-
+
+#if 0
 void btHingeConstraint::testLimit()
 {
@@ -660,7 +602,403 @@
 	}
 	return;
-} // btHingeConstraint::testLimit()
-
-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
+}
+#else
+
+
+void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
+{
+	// Compute limit information
+	m_hingeAngle = getHingeAngle(transA,transB);
+	m_correction = btScalar(0.);
+	m_limitSign = btScalar(0.);
+	m_solveLimit = false;
+	if (m_lowerLimit <= m_upperLimit)
+	{
+		m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit);
+		if (m_hingeAngle <= m_lowerLimit)
+		{
+			m_correction = (m_lowerLimit - m_hingeAngle);
+			m_limitSign = 1.0f;
+			m_solveLimit = true;
+		} 
+		else if (m_hingeAngle >= m_upperLimit)
+		{
+			m_correction = m_upperLimit - m_hingeAngle;
+			m_limitSign = -1.0f;
+			m_solveLimit = true;
+		}
+	}
+	return;
+}
+#endif
+
+static btVector3 vHinge(0, 0, btScalar(1));
+
+void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
+{
+	// convert target from body to constraint space
+	btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
+	qConstraint.normalize();
+
+	// extract "pure" hinge component
+	btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize();
+	btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
+	btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
+	qHinge.normalize();
+
+	// compute angular target, clamped to limits
+	btScalar targetAngle = qHinge.getAngle();
+	if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
+	{
+		qHinge = operator-(qHinge);
+		targetAngle = qHinge.getAngle();
+	}
+	if (qHinge.getZ() < 0)
+		targetAngle = -targetAngle;
+
+	setMotorTarget(targetAngle, dt);
+}
+
+void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
+{
+	if (m_lowerLimit < m_upperLimit)
+	{
+		if (targetAngle < m_lowerLimit)
+			targetAngle = m_lowerLimit;
+		else if (targetAngle > m_upperLimit)
+			targetAngle = m_upperLimit;
+	}
+
+	// compute angular velocity
+	btScalar curAngle  = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+	btScalar dAngle = targetAngle - curAngle;
+	m_motorTargetVelocity = dAngle / dt;
+}
+
+
+
+void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+{
+	btAssert(!m_useSolveConstraintObsolete);
+	int i, s = info->rowskip;
+	// transforms in world space
+	btTransform trA = transA*m_rbAFrame;
+	btTransform trB = transB*m_rbBFrame;
+	// pivot point
+	btVector3 pivotAInW = trA.getOrigin();
+	btVector3 pivotBInW = trB.getOrigin();
+#if 1
+	// difference between frames in WCS
+	btVector3 ofs = trB.getOrigin() - trA.getOrigin();
+	// now get weight factors depending on masses
+	btScalar miA = getRigidBodyA().getInvMass();
+	btScalar miB = getRigidBodyB().getInvMass();
+	bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
+	btScalar miS = miA + miB;
+	btScalar factA, factB;
+	if(miS > btScalar(0.f))
+	{
+		factA = miB / miS;
+	}
+	else 
+	{
+		factA = btScalar(0.5f);
+	}
+	factB = btScalar(1.0f) - factA;
+	// get the desired direction of hinge axis
+	// as weighted sum of Z-orthos of frameA and frameB in WCS
+	btVector3 ax1A = trA.getBasis().getColumn(2);
+	btVector3 ax1B = trB.getBasis().getColumn(2);
+	btVector3 ax1 = ax1A * factA + ax1B * factB;
+	ax1.normalize();
+	// fill first 3 rows 
+	// we want: velA + wA x relA == velB + wB x relB
+	btTransform bodyA_trans = transA;
+	btTransform bodyB_trans = transB;
+	int s0 = 0;
+	int s1 = s;
+	int s2 = s * 2;
+	int nrow = 2; // last filled row
+	btVector3 tmpA, tmpB, relA, relB, p, q;
+	// get vector from bodyB to frameB in WCS
+	relB = trB.getOrigin() - bodyB_trans.getOrigin();
+	// get its projection to hinge axis
+	btVector3 projB = ax1 * relB.dot(ax1);
+	// get vector directed from bodyB to hinge axis (and orthogonal to it)
+	btVector3 orthoB = relB - projB;
+	// same for bodyA
+	relA = trA.getOrigin() - bodyA_trans.getOrigin();
+	btVector3 projA = ax1 * relA.dot(ax1);
+	btVector3 orthoA = relA - projA;
+	btVector3 totalDist = projA - projB;
+	// get offset vectors relA and relB
+	relA = orthoA + totalDist * factA;
+	relB = orthoB - totalDist * factB;
+	// now choose average ortho to hinge axis
+	p = orthoB * factA + orthoA * factB;
+	btScalar len2 = p.length2();
+	if(len2 > SIMD_EPSILON)
+	{
+		p /= btSqrt(len2);
+	}
+	else
+	{
+		p = trA.getBasis().getColumn(1);
+	}
+	// make one more ortho
+	q = ax1.cross(p);
+	// fill three rows
+	tmpA = relA.cross(p);
+	tmpB = relB.cross(p);
+    for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
+    for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
+	tmpA = relA.cross(q);
+	tmpB = relB.cross(q);
+	if(hasStaticBody && getSolveLimit())
+	{ // to make constraint between static and dynamic objects more rigid
+		// remove wA (or wB) from equation if angular limit is hit
+		tmpB *= factB;
+		tmpA *= factA;
+	}
+	for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
+    for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
+	tmpA = relA.cross(ax1);
+	tmpB = relB.cross(ax1);
+	if(hasStaticBody)
+	{ // to make constraint between static and dynamic objects more rigid
+		// remove wA (or wB) from equation
+		tmpB *= factB;
+		tmpA *= factA;
+	}
+	for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
+    for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
+
+	btScalar k = info->fps * info->erp;
+
+	if (!m_angularOnly)
+	{
+		for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
+		for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
+		for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
+	
+	// compute three elements of right hand side
+	
+		btScalar rhs = k * p.dot(ofs);
+		info->m_constraintError[s0] = rhs;
+		rhs = k * q.dot(ofs);
+		info->m_constraintError[s1] = rhs;
+		rhs = k * ax1.dot(ofs);
+		info->m_constraintError[s2] = rhs;
+	}
+	// the hinge axis should be the only unconstrained
+	// rotational axis, the angular velocity of the two bodies perpendicular to
+	// the hinge axis should be equal. thus the constraint equations are
+	//    p*w1 - p*w2 = 0
+	//    q*w1 - q*w2 = 0
+	// where p and q are unit vectors normal to the hinge axis, and w1 and w2
+	// are the angular velocity vectors of the two bodies.
+	int s3 = 3 * s;
+	int s4 = 4 * s;
+	info->m_J1angularAxis[s3 + 0] = p[0];
+	info->m_J1angularAxis[s3 + 1] = p[1];
+	info->m_J1angularAxis[s3 + 2] = p[2];
+	info->m_J1angularAxis[s4 + 0] = q[0];
+	info->m_J1angularAxis[s4 + 1] = q[1];
+	info->m_J1angularAxis[s4 + 2] = q[2];
+
+	info->m_J2angularAxis[s3 + 0] = -p[0];
+	info->m_J2angularAxis[s3 + 1] = -p[1];
+	info->m_J2angularAxis[s3 + 2] = -p[2];
+	info->m_J2angularAxis[s4 + 0] = -q[0];
+	info->m_J2angularAxis[s4 + 1] = -q[1];
+	info->m_J2angularAxis[s4 + 2] = -q[2];
+	// compute the right hand side of the constraint equation. set relative
+	// body velocities along p and q to bring the hinge back into alignment.
+	// if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
+	// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
+	// if "theta" is the angle between ax1 and ax2, we need an angular velocity
+	// along u to cover angle erp*theta in one step :
+	//   |angular_velocity| = angle/time = erp*theta / stepsize
+	//                      = (erp*fps) * theta
+	//    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+	//                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+	// ...as ax1 and ax2 are unit length. if theta is smallish,
+	// theta ~= sin(theta), so
+	//    angular_velocity  = (erp*fps) * (ax1 x ax2)
+	// ax1 x ax2 is in the plane space of ax1, so we project the angular
+	// velocity to p and q to find the right hand side.
+	k = info->fps * info->erp;
+	btVector3 u = ax1A.cross(ax1B);
+	info->m_constraintError[s3] = k * u.dot(p);
+	info->m_constraintError[s4] = k * u.dot(q);
+#endif
+	// check angular limits
+	nrow = 4; // last filled row
+	int srow;
+	btScalar limit_err = btScalar(0.0);
+	int limit = 0;
+	if(getSolveLimit())
+	{
+		limit_err = m_correction * m_referenceSign;
+		limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+	}
+	// if the hinge has joint limits or motor, add in the extra row
+	int powered = 0;
+	if(getEnableAngularMotor())
+	{
+		powered = 1;
+	}
+	if(limit || powered) 
+	{
+		nrow++;
+		srow = nrow * info->rowskip;
+		info->m_J1angularAxis[srow+0] = ax1[0];
+		info->m_J1angularAxis[srow+1] = ax1[1];
+		info->m_J1angularAxis[srow+2] = ax1[2];
+
+		info->m_J2angularAxis[srow+0] = -ax1[0];
+		info->m_J2angularAxis[srow+1] = -ax1[1];
+		info->m_J2angularAxis[srow+2] = -ax1[2];
+
+		btScalar lostop = getLowerLimit();
+		btScalar histop = getUpperLimit();
+		if(limit && (lostop == histop))
+		{  // the joint motor is ineffective
+			powered = 0;
+		}
+		info->m_constraintError[srow] = btScalar(0.0f);
+		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
+		if(powered)
+		{
+			if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
+			{
+				info->cfm[srow] = m_normalCFM;
+			}
+			btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
+			info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
+			info->m_lowerLimit[srow] = - m_maxMotorImpulse;
+			info->m_upperLimit[srow] =   m_maxMotorImpulse;
+		}
+		if(limit)
+		{
+			k = info->fps * currERP;
+			info->m_constraintError[srow] += k * limit_err;
+			if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
+			{
+				info->cfm[srow] = m_stopCFM;
+			}
+			if(lostop == histop) 
+			{
+				// limited low and high simultaneously
+				info->m_lowerLimit[srow] = -SIMD_INFINITY;
+				info->m_upperLimit[srow] = SIMD_INFINITY;
+			}
+			else if(limit == 1) 
+			{ // low limit
+				info->m_lowerLimit[srow] = 0;
+				info->m_upperLimit[srow] = SIMD_INFINITY;
+			}
+			else 
+			{ // high limit
+				info->m_lowerLimit[srow] = -SIMD_INFINITY;
+				info->m_upperLimit[srow] = 0;
+			}
+			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
+			btScalar bounce = m_relaxationFactor;
+			if(bounce > btScalar(0.0))
+			{
+				btScalar vel = angVelA.dot(ax1);
+				vel -= angVelB.dot(ax1);
+				// only apply bounce if the velocity is incoming, and if the
+				// resulting c[] exceeds what we already have.
+				if(limit == 1)
+				{	// low limit
+					if(vel < 0)
+					{
+						btScalar newc = -bounce * vel;
+						if(newc > info->m_constraintError[srow])
+						{
+							info->m_constraintError[srow] = newc;
+						}
+					}
+				}
+				else
+				{	// high limit - all those computations are reversed
+					if(vel > 0)
+					{
+						btScalar newc = -bounce * vel;
+						if(newc < info->m_constraintError[srow])
+						{
+							info->m_constraintError[srow] = newc;
+						}
+					}
+				}
+			}
+			info->m_constraintError[srow] *= m_biasFactor;
+		} // if(limit)
+	} // if angular limit or powered
+}
+
+
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+///If no axis is provided, it uses the default axis for this constraint.
+void btHingeConstraint::setParam(int num, btScalar value, int axis)
+{
+	if((axis == -1) || (axis == 5))
+	{
+		switch(num)
+		{	
+			case BT_CONSTRAINT_STOP_ERP :
+				m_stopERP = value;
+				m_flags |= BT_HINGE_FLAGS_ERP_STOP;
+				break;
+			case BT_CONSTRAINT_STOP_CFM :
+				m_stopCFM = value;
+				m_flags |= BT_HINGE_FLAGS_CFM_STOP;
+				break;
+			case BT_CONSTRAINT_CFM :
+				m_normalCFM = value;
+				m_flags |= BT_HINGE_FLAGS_CFM_NORM;
+				break;
+			default : 
+				btAssertConstrParams(0);
+		}
+	}
+	else
+	{
+		btAssertConstrParams(0);
+	}
+}
+
+///return the local value of parameter
+btScalar btHingeConstraint::getParam(int num, int axis) const 
+{
+	btScalar retVal = 0;
+	if((axis == -1) || (axis == 5))
+	{
+		switch(num)
+		{	
+			case BT_CONSTRAINT_STOP_ERP :
+				btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
+				retVal = m_stopERP;
+				break;
+			case BT_CONSTRAINT_STOP_CFM :
+				btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
+				retVal = m_stopCFM;
+				break;
+			case BT_CONSTRAINT_CFM :
+				btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
+				retVal = m_normalCFM;
+				break;
+			default : 
+				btAssertConstrParams(0);
+		}
+	}
+	else
+	{
+		btAssertConstrParams(0);
+	}
+	return retVal;
+}
+
+
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h	(revision 8284)
@@ -25,7 +25,24 @@
 class btRigidBody;
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btHingeConstraintData	btHingeConstraintDoubleData
+#define btHingeConstraintDataName	"btHingeConstraintDoubleData"
+#else
+#define btHingeConstraintData	btHingeConstraintFloatData
+#define btHingeConstraintDataName	"btHingeConstraintFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
+
+
+enum btHingeFlags
+{
+	BT_HINGE_FLAGS_CFM_STOP = 1,
+	BT_HINGE_FLAGS_ERP_STOP = 2,
+	BT_HINGE_FLAGS_CFM_NORM = 4
+};
+
+
 /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
 /// axis defines the orientation of the hinge axis
-class btHingeConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
 {
 #ifdef IN_PARALLELL_SOLVER
@@ -61,12 +78,20 @@
 	bool		m_solveLimit;
 	bool		m_useSolveConstraintObsolete;
+	bool		m_useOffsetForConstraintFrame;
 	bool		m_useReferenceFrameA;
 
+	btScalar	m_accMotorImpulse;
+
+	int			m_flags;
+	btScalar	m_normalCFM;
+	btScalar	m_stopCFM;
+	btScalar	m_stopERP;
+
 	
 public:
 
-	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA = false);
-
-	btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false);
+	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false);
+
+	btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false);
 	
 	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
@@ -74,5 +99,4 @@
 	btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
 
-	btHingeConstraint();
 
 	virtual void	buildJacobian();
@@ -80,7 +104,13 @@
 	virtual void getInfo1 (btConstraintInfo1* info);
 
+	void getInfo1NonVirtual(btConstraintInfo1* info);
+
 	virtual void getInfo2 (btConstraintInfo2* info);
-	
-	virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
+
+	void	getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
+
+	void	getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
+	void	getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
+		
 
 	void	updateRHS(btScalar	timeStep);
@@ -117,8 +147,17 @@
 	}
 
+	// extra motor API, including ability to set a target rotation (as opposed to angular velocity)
+	// note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to
+	//       maintain a given angular target.
+	void enableMotor(bool enableMotor) 	{ m_enableAngularMotor = enableMotor; }
+	void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
+	void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
+	void setMotorTarget(btScalar targetAngle, btScalar dt);
+
+
 	void	setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
 	{
-		m_lowerLimit = low;
-		m_upperLimit = high;
+		m_lowerLimit = btNormalizeAngle(low);
+		m_upperLimit = btNormalizeAngle(high);
 
 		m_limitSoftness =  _softness;
@@ -128,4 +167,27 @@
 	}
 
+	void	setAxis(btVector3& axisInA)
+	{
+		btVector3 rbAxisA1, rbAxisA2;
+		btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
+		btVector3 pivotInA = m_rbAFrame.getOrigin();
+//		m_rbAFrame.getOrigin() = pivotInA;
+		m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
+										rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
+										rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+
+		btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA;
+
+		btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
+		btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
+		btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
+
+
+		m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(pivotInA);
+		m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
+										rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
+										rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
+	}
+
 	btScalar	getLowerLimit() const
 	{
@@ -141,9 +203,14 @@
 	btScalar getHingeAngle();
 
-	void testLimit();
-
-
-	const btTransform& getAFrame() { return m_rbAFrame; };	
-	const btTransform& getBFrame() { return m_rbBFrame; };
+	btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
+
+	void testLimit(const btTransform& transA,const btTransform& transB);
+
+
+	const btTransform& getAFrame() const { return m_rbAFrame; };	
+	const btTransform& getBFrame() const { return m_rbBFrame; };
+
+	btTransform& getAFrame() { return m_rbAFrame; };	
+	btTransform& getBFrame() { return m_rbBFrame; };
 
 	inline int getSolveLimit()
@@ -173,6 +240,93 @@
 		return m_maxMotorImpulse; 
 	}
+	// access for UseFrameOffset
+	bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
+	void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
+
+
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///If no axis is provided, it uses the default axis for this constraint.
+	virtual	void	setParam(int num, btScalar value, int axis = -1);
+	///return the local value of parameter
+	virtual	btScalar getParam(int num, int axis = -1) const;
+
+	virtual	int	calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+
 
 };
 
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btHingeConstraintDoubleData
+{
+	btTypedConstraintData	m_typeConstraintData;
+	btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTransformDoubleData m_rbBFrame;
+	int			m_useReferenceFrameA;
+	int			m_angularOnly;
+	int			m_enableAngularMotor;
+	float	m_motorTargetVelocity;
+	float	m_maxMotorImpulse;
+
+	float	m_lowerLimit;
+	float	m_upperLimit;
+	float	m_limitSoftness;
+	float	m_biasFactor;
+	float	m_relaxationFactor;
+
+};
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btHingeConstraintFloatData
+{
+	btTypedConstraintData	m_typeConstraintData;
+	btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTransformFloatData m_rbBFrame;
+	int			m_useReferenceFrameA;
+	int			m_angularOnly;
+	
+	int			m_enableAngularMotor;
+	float	m_motorTargetVelocity;
+	float	m_maxMotorImpulse;
+
+	float	m_lowerLimit;
+	float	m_upperLimit;
+	float	m_limitSoftness;
+	float	m_biasFactor;
+	float	m_relaxationFactor;
+
+};
+
+
+
+SIMD_FORCE_INLINE	int	btHingeConstraint::calculateSerializeBufferSize() const
+{
+	return sizeof(btHingeConstraintData);
+}
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE	const char*	btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer;
+	btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer);
+
+	m_rbAFrame.serialize(hingeData->m_rbAFrame);
+	m_rbBFrame.serialize(hingeData->m_rbBFrame);
+
+	hingeData->m_angularOnly = m_angularOnly;
+	hingeData->m_enableAngularMotor = m_enableAngularMotor;
+	hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
+	hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
+	hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
+	
+	hingeData->m_lowerLimit = float(m_lowerLimit);
+	hingeData->m_upperLimit = float(m_upperLimit);
+	hingeData->m_limitSoftness = float(m_limitSoftness);
+	hingeData->m_biasFactor = float(m_biasFactor);
+	hingeData->m_relaxationFactor = float(m_relaxationFactor);
+
+	return btHingeConstraintDataName;
+}
+
 #endif //HINGECONSTRAINT_H
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btJacobianEntry.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btJacobianEntry.h	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btJacobianEntry.h	(revision 8284)
@@ -29,5 +29,5 @@
 /// it can be used in combination with a constraint solver
 /// Can be used to relate the effect of an impulse to the constraint error
-class btJacobianEntry
+ATTRIBUTE_ALIGNED16(class) btJacobianEntry
 {
 public:
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp	(revision 8284)
@@ -21,12 +21,9 @@
 
 
-btPoint2PointConstraint::btPoint2PointConstraint()
-:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE),
-m_useSolveConstraintObsolete(false)
-{
-}
+
 
 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
+m_flags(0),
 m_useSolveConstraintObsolete(false)
 {
@@ -37,4 +34,5 @@
 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
+m_flags(0),
 m_useSolveConstraintObsolete(false)
 {
@@ -44,4 +42,5 @@
 void	btPoint2PointConstraint::buildJacobian()
 {
+
 	///we need it for both methods
 	{
@@ -67,8 +66,13 @@
 	}
 
-}
-
+
+}
 
 void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
+{
+	getInfo1NonVirtual(info);
+}
+
+void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
 {
 	if (m_useSolveConstraintObsolete)
@@ -83,13 +87,17 @@
 }
 
+
+
+
 void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
 {
+	getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+
+void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
+{
 	btAssert(!m_useSolveConstraintObsolete);
 
 	 //retrieve matrices
-	btTransform body0_trans;
-	body0_trans = m_rbA.getCenterOfMassTransform();
-    btTransform body1_trans;
-	body1_trans = m_rbB.getCenterOfMassTransform();
 
 	// anchor points in global coordinates with respect to body PORs.
@@ -97,6 +105,6 @@
     // set jacobian
     info->m_J1linearAxis[0] = 1;
-    info->m_J1linearAxis[info->rowskip+1] = 1;
-    info->m_J1linearAxis[2*info->rowskip+2] = 1;
+	info->m_J1linearAxis[info->rowskip+1] = 1;
+	info->m_J1linearAxis[2*info->rowskip+2] = 1;
 
 	btVector3 a1 = body0_trans.getBasis()*getPivotInA();
@@ -127,12 +135,19 @@
 
     // set right hand side
-    btScalar k = info->fps * info->erp;
+	btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
+    btScalar k = info->fps * currERP;
     int j;
-
 	for (j=0; j<3; j++)
     {
-        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] -                     a1[j] - body0_trans.getOrigin()[j]);
+        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
 		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
     }
+	if(m_flags & BT_P2P_FLAGS_CFM)
+	{
+		for (j=0; j<3; j++)
+		{
+			info->cfm[j*info->rowskip] = m_cfm;
+		}
+	}
 
 	btScalar impulseClamp = m_setting.m_impulseClamp;//
@@ -145,81 +160,9 @@
 		}
 	}
-	
-}
-
-
-void	btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
-{
-	if (m_useSolveConstraintObsolete)
-	{
-		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
-		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
-
-
-		btVector3 normal(0,0,0);
-		
-
-	//	btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-	//	btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
-
-		for (int i=0;i<3;i++)
-		{		
-			normal[i] = 1;
-			btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
-
-			btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
-			btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-			//this jacobian entry could be re-used for all iterations
-			
-			btVector3 vel1,vel2;
-			bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
-			bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
-			btVector3 vel = vel1 - vel2;
-			
-			btScalar rel_vel;
-			rel_vel = normal.dot(vel);
-
-		/*
-			//velocity error (first order error)
-			btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
-															m_rbB.getLinearVelocity(),angvelB);
-		*/
-		
-			//positional error (zeroth order error)
-			btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
-			
-			btScalar deltaImpulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
-
-			btScalar impulseClamp = m_setting.m_impulseClamp;
-			
-			const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse;
-			if (sum < -impulseClamp)
-			{
-				deltaImpulse = -impulseClamp-m_appliedImpulse;
-				m_appliedImpulse = -impulseClamp;
-			}
-			else if (sum > impulseClamp) 
-			{
-				deltaImpulse = impulseClamp-m_appliedImpulse;
-				m_appliedImpulse = impulseClamp;
-			}
-			else
-			{
-				m_appliedImpulse = sum;
-			}
-
-			
-			btVector3 impulse_vector = normal * deltaImpulse;
-			
-			btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
-			btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
-			bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse);
-			bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse);
-
-
-			normal[i] = 0;
-		}
-	}
-}
+	info->m_damping = m_setting.m_damping;
+	
+}
+
+
 
 void	btPoint2PointConstraint::updateRHS(btScalar	timeStep)
@@ -229,2 +172,59 @@
 }
 
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+///If no axis is provided, it uses the default axis for this constraint.
+void btPoint2PointConstraint::setParam(int num, btScalar value, int axis)
+{
+	if(axis != -1)
+	{
+		btAssertConstrParams(0);
+	}
+	else
+	{
+		switch(num)
+		{
+			case BT_CONSTRAINT_ERP :
+			case BT_CONSTRAINT_STOP_ERP :
+				m_erp = value; 
+				m_flags |= BT_P2P_FLAGS_ERP;
+				break;
+			case BT_CONSTRAINT_CFM :
+			case BT_CONSTRAINT_STOP_CFM :
+				m_cfm = value; 
+				m_flags |= BT_P2P_FLAGS_CFM;
+				break;
+			default: 
+				btAssertConstrParams(0);
+		}
+	}
+}
+
+///return the local value of parameter
+btScalar btPoint2PointConstraint::getParam(int num, int axis) const 
+{
+	btScalar retVal(SIMD_INFINITY);
+	if(axis != -1)
+	{
+		btAssertConstrParams(0);
+	}
+	else
+	{
+		switch(num)
+		{
+			case BT_CONSTRAINT_ERP :
+			case BT_CONSTRAINT_STOP_ERP :
+				btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
+				retVal = m_erp; 
+				break;
+			case BT_CONSTRAINT_CFM :
+			case BT_CONSTRAINT_STOP_CFM :
+				btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
+				retVal = m_cfm; 
+				break;
+			default: 
+				btAssertConstrParams(0);
+		}
+	}
+	return retVal;
+}
+	
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h	(revision 8284)
@@ -23,4 +23,13 @@
 class btRigidBody;
 
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btPoint2PointConstraintData	btPoint2PointConstraintDoubleData
+#define btPoint2PointConstraintDataName	"btPoint2PointConstraintDoubleData"
+#else
+#define btPoint2PointConstraintData	btPoint2PointConstraintFloatData
+#define btPoint2PointConstraintDataName	"btPoint2PointConstraintFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
+
 struct	btConstraintSetting
 {
@@ -36,6 +45,12 @@
 };
 
+enum btPoint2PointFlags
+{
+	BT_P2P_FLAGS_ERP = 1,
+	BT_P2P_FLAGS_CFM = 2
+};
+
 /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
-class btPoint2PointConstraint : public btTypedConstraint
+ATTRIBUTE_ALIGNED16(class) btPoint2PointConstraint : public btTypedConstraint
 {
 #ifdef IN_PARALLELL_SOLVER
@@ -47,5 +62,7 @@
 	btVector3	m_pivotInB;
 	
-	
+	int			m_flags;
+	btScalar	m_erp;
+	btScalar	m_cfm;
 	
 public:
@@ -60,5 +77,4 @@
 	btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA);
 
-	btPoint2PointConstraint();
 
 	virtual void	buildJacobian();
@@ -66,8 +82,9 @@
 	virtual void getInfo1 (btConstraintInfo1* info);
 
+	void getInfo1NonVirtual (btConstraintInfo1* info);
+
 	virtual void getInfo2 (btConstraintInfo2* info);
 
-
-	virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
+	void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans);
 
 	void	updateRHS(btScalar	timeStep);
@@ -93,6 +110,52 @@
 	}
 
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///If no axis is provided, it uses the default axis for this constraint.
+	virtual	void	setParam(int num, btScalar value, int axis = -1);
+	///return the local value of parameter
+	virtual	btScalar getParam(int num, int axis = -1) const;
+
+	virtual	int	calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+
 
 };
 
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btPoint2PointConstraintFloatData
+{
+	btTypedConstraintData	m_typeConstraintData;
+	btVector3FloatData	m_pivotInA;
+	btVector3FloatData	m_pivotInB;
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btPoint2PointConstraintDoubleData
+{
+	btTypedConstraintData	m_typeConstraintData;
+	btVector3DoubleData	m_pivotInA;
+	btVector3DoubleData	m_pivotInB;
+};
+
+
+SIMD_FORCE_INLINE	int	btPoint2PointConstraint::calculateSerializeBufferSize() const
+{
+	return sizeof(btPoint2PointConstraintData);
+
+}
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE	const char*	btPoint2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btPoint2PointConstraintData* p2pData = (btPoint2PointConstraintData*)dataBuffer;
+
+	btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
+	m_pivotInA.serialize(p2pData->m_pivotInA);
+	m_pivotInB.serialize(p2pData->m_pivotInB);
+
+	return btPoint2PointConstraintDataName;
+}
+
 #endif //POINT2POINTCONSTRAINT_H
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp	(revision 8284)
@@ -35,4 +35,6 @@
 #include <string.h> //for memset
 
+int		gNumSplitImpulseRecoveries = 0;
+
 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
 :m_btSeed2(0)
@@ -56,5 +58,5 @@
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
-void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
 {
 #ifdef USE_SIMD
@@ -63,6 +65,6 @@
 	__m128	upperLimit1 = _mm_set1_ps(c.m_upperLimit);
 	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
-	__m128 deltaVel1Dotn	=	_mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
-	__m128 deltaVel2Dotn	=	_mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
+	__m128 deltaVel1Dotn	=	_mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
+	__m128 deltaVel2Dotn	=	_mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
 	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
 	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
@@ -77,11 +79,11 @@
 	deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
 	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
-	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
-	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
+	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
+	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
 	__m128 impulseMagnitude = deltaImpulse;
-	body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
-	body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
-	body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
-	body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+	body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
 #else
 	resolveSingleConstraintRowGeneric(body1,body2,c);
@@ -90,11 +92,11 @@
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
 {
 	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
-	const btScalar deltaVel1Dotn	=	c.m_contactNormal.dot(body1.m_deltaLinearVelocity) 	+ c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
-	const btScalar deltaVel2Dotn	=	-c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
-
-	const btScalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;
+	const btScalar deltaVel1Dotn	=	c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
+	const btScalar deltaVel2Dotn	=	-c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
+
+//	const btScalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;
 	deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
 	deltaImpulse	-=	deltaVel2Dotn*c.m_jacDiagABInv;
@@ -115,11 +117,9 @@
 		c.m_appliedImpulse = sum;
 	}
-	if (body1.m_invMass)
-		body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
-	if (body2.m_invMass)
-		body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
-}
-
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+		body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
+		body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+}
+
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
 {
 #ifdef USE_SIMD
@@ -128,6 +128,6 @@
 	__m128	upperLimit1 = _mm_set1_ps(c.m_upperLimit);
 	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
-	__m128 deltaVel1Dotn	=	_mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
-	__m128 deltaVel2Dotn	=	_mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
+	__m128 deltaVel1Dotn	=	_mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
+	__m128 deltaVel2Dotn	=	_mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
 	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
 	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
@@ -139,11 +139,11 @@
 	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
 	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
-	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
-	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
+	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
+	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
 	__m128 impulseMagnitude = deltaImpulse;
-	body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
-	body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
-	body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
-	body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+	body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
 #else
 	resolveSingleConstraintRowLowerLimit(body1,body2,c);
@@ -152,9 +152,9 @@
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
 {
 	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
-	const btScalar deltaVel1Dotn	=	c.m_contactNormal.dot(body1.m_deltaLinearVelocity) 	+ c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
-	const btScalar deltaVel2Dotn	=	-c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
+	const btScalar deltaVel1Dotn	=	c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
+	const btScalar deltaVel2Dotn	=	-c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
 
 	deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
@@ -170,8 +170,71 @@
 		c.m_appliedImpulse = sum;
 	}
-	if (body1.m_invMass)
-		body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
-	if (body2.m_invMass)
-		body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
+	body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
+	body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+}
+
+
+void	btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
+        btRigidBody& body1,
+        btRigidBody& body2,
+        const btSolverConstraint& c)
+{
+		if (c.m_rhsPenetration)
+        {
+			gNumSplitImpulseRecoveries++;
+			btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
+			const btScalar deltaVel1Dotn	=	c.m_contactNormal.dot(body1.internalGetPushVelocity()) 	+ c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
+			const btScalar deltaVel2Dotn	=	-c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
+
+			deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
+			deltaImpulse	-=	deltaVel2Dotn*c.m_jacDiagABInv;
+			const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
+			if (sum < c.m_lowerLimit)
+			{
+				deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
+				c.m_appliedPushImpulse = c.m_lowerLimit;
+			}
+			else
+			{
+				c.m_appliedPushImpulse = sum;
+			}
+			body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
+			body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+        }
+}
+
+ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
+{
+#ifdef USE_SIMD
+	if (!c.m_rhsPenetration)
+		return;
+
+	gNumSplitImpulseRecoveries++;
+
+	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
+	__m128	lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+	__m128	upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm)));
+	__m128 deltaVel1Dotn	=	_mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
+	__m128 deltaVel2Dotn	=	_mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128));
+	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+	btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
+	btSimdScalar resultLowerLess,resultUpperLess;
+	resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
+	resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
+	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
+	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
+	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
+	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
+	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
+	__m128 impulseMagnitude = deltaImpulse;
+	body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+	body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+	body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+	body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+#else
+	resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
+#endif
 }
 
@@ -215,27 +278,31 @@
 
 
-
+#if 0
 void	btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
 {
 	btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
 
-	solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
-	solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
+	solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
+	solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
+	solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
+	solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
 
 	if (rb)
 	{
-		solverBody->m_invMass = rb->getInvMass();
+		solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor();
 		solverBody->m_originalBody = rb;
 		solverBody->m_angularFactor = rb->getAngularFactor();
 	} else
 	{
-		solverBody->m_invMass = 0.f;
+		solverBody->internalGetInvMass().setValue(0,0,0);
 		solverBody->m_originalBody = 0;
-		solverBody->m_angularFactor = 1.f;
-	}
-}
-
-
-int		gNumSplitImpulseRecoveries = 0;
+		solverBody->m_angularFactor.setValue(1,1,1);
+	}
+}
+#endif
+
+
+
+
 
 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
@@ -263,6 +330,5 @@
 
 
-
-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)
+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)
 {
 
@@ -271,11 +337,8 @@
 	btRigidBody* body1=btRigidBody::upcast(colObj1);
 
-	btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
-	memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
 	solverConstraint.m_contactNormal = normalAxis;
 
-	solverConstraint.m_solverBodyIdA = solverBodyIdA;
-	solverConstraint.m_solverBodyIdB = solverBodyIdB;
-	solverConstraint.m_frictionIndex = frictionIndex;
+	solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody();
+	solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody();
 
 	solverConstraint.m_friction = cp.m_combinedFriction;
@@ -283,5 +346,5 @@
 
 	solverConstraint.m_appliedImpulse = 0.f;
-	//	solverConstraint.m_appliedPushImpulse = 0.f;
+	solverConstraint.m_appliedPushImpulse = 0.f;
 
 	{
@@ -338,14 +401,23 @@
 		rel_vel = vel1Dotn+vel2Dotn;
 
-		btScalar positionalError = 0.f;
-
-		btSimdScalar velocityError =  - rel_vel;
+//		btScalar positionalError = 0.f;
+
+		btSimdScalar velocityError =  desiredVelocity - rel_vel;
 		btSimdScalar	velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
 		solverConstraint.m_rhs = velocityImpulse;
-		solverConstraint.m_cfm = 0.f;
+		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& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
+	solverConstraint.m_frictionIndex = frictionIndex;
+	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2, 
+							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
@@ -353,4 +425,5 @@
 int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
 {
+#if 0
 	int solverBodyIdA = -1;
 
@@ -374,68 +447,34 @@
 	}
 	return solverBodyIdA;
+#endif
+	return 0;
 }
 #include <stdio.h>
 
 
-
-void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
-{
-	btCollisionObject* colObj0=0,*colObj1=0;
-
-	colObj0 = (btCollisionObject*)manifold->getBody0();
-	colObj1 = (btCollisionObject*)manifold->getBody1();
-
-	int solverBodyIdA=-1;
-	int solverBodyIdB=-1;
-
-	if (manifold->getNumContacts())
-	{
-		solverBodyIdA = getOrInitSolverBody(*colObj0);
-		solverBodyIdB = getOrInitSolverBody(*colObj1);
-	}
-
-	///avoid collision response between two static objects
-	if (!solverBodyIdA && !solverBodyIdB)
-		return;
-
-	btVector3 rel_pos1;
-	btVector3 rel_pos2;
-	btScalar relaxation;
-
-	for (int j=0;j<manifold->getNumContacts();j++)
-	{
-
-		btManifoldPoint& cp = manifold->getContactPoint(j);
-
-		if (cp.getDistance() <= manifold->getContactProcessingThreshold())
-		{
+void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, 
+																 btCollisionObject* colObj0, btCollisionObject* colObj1,
+																 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();
 
+//			btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
+//			btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
 			rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
 			rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
 
-
 			relaxation = 1.f;
-			btScalar rel_vel;
-			btVector3 vel;
-
-			int frictionIndex = m_tmpSolverContactConstraintPool.size();
-
-			{
-				btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
-				btRigidBody* rb0 = btRigidBody::upcast(colObj0);
-				btRigidBody* rb1 = btRigidBody::upcast(colObj1);
-
-				solverConstraint.m_solverBodyIdA = solverBodyIdA;
-				solverConstraint.m_solverBodyIdB = solverBodyIdB;
-
-				solverConstraint.m_originalContactPoint = &cp;
-
-				btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
-				solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
-				btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
-				solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+
+			btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+			solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+			btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
+			solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+
 				{
 #ifdef COMPUTE_IMPULSE_DENOM
@@ -467,10 +506,10 @@
 
 
-				btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : 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);
+
+
+			btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : 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;
@@ -499,7 +538,7 @@
 					solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
 					if (rb0)
-						m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+						rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
 					if (rb1)
-						m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
+						rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
 				} else
 				{
@@ -507,5 +546,5 @@
 				}
 
-				//							solverConstraint.m_appliedPushImpulse = 0.f;
+				solverConstraint.m_appliedPushImpulse = 0.f;
 
 				{
@@ -523,5 +562,15 @@
 					btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
 					btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
-					solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+					if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+					{
+						//combine position and velocity into rhs
+						solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+						solverConstraint.m_rhsPenetration = 0.f;
+					} else
+					{
+						//split position and velocity into rhs and m_rhsPenetration
+						solverConstraint.m_rhs = velocityImpulse;
+						solverConstraint.m_rhsPenetration = penetrationImpulse;
+					}
 					solverConstraint.m_cfm = 0.f;
 					solverConstraint.m_lowerLimit = 0;
@@ -530,54 +579,14 @@
 
 
-				/////setup the friction constraints
-
-
-
-				if (1)
-				{
-					solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
-					if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
-					{
-						cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
-						btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
-						if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
-						{
-							cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
-							applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
-							applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
-							addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-							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,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
-							btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
-							applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
-							applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
-
-							addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-							if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-							{
-								applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
-								applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
-								addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-							}
-							cp.m_lateralFrictionInitialized = true;
-						}
-
-					} else
-					{
-						addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-						if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-							addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-					}
-
+
+
+}
+
+
+
+void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, 
+																		btRigidBody* rb0, btRigidBody* rb1, 
+																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
+{
 					if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
 					{
@@ -588,7 +597,7 @@
 								frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
 								if (rb0)
-									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
+									rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
 								if (rb1)
-									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
+									rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
 							} else
 							{
@@ -604,7 +613,7 @@
 								frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
 								if (rb0)
-									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
+									rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
 								if (rb1)
-									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
+									rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
 							} else
 							{
@@ -622,14 +631,107 @@
 						}
 					}
+}
+
+
+
+
+void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
+{
+	btCollisionObject* colObj0=0,*colObj1=0;
+
+	colObj0 = (btCollisionObject*)manifold->getBody0();
+	colObj1 = (btCollisionObject*)manifold->getBody1();
+
+
+	btRigidBody* solverBodyA = btRigidBody::upcast(colObj0);
+	btRigidBody* solverBodyB = btRigidBody::upcast(colObj1);
+
+	///avoid collision response between two static objects
+	if ((!solverBodyA || !solverBodyA->getInvMass()) && (!solverBodyB || !solverBodyB->getInvMass()))
+		return;
+
+	for (int j=0;j<manifold->getNumContacts();j++)
+	{
+
+		btManifoldPoint& cp = manifold->getContactPoint(j);
+
+		if (cp.getDistance() <= manifold->getContactProcessingThreshold())
+		{
+			btVector3 rel_pos1;
+			btVector3 rel_pos2;
+			btScalar relaxation;
+			btScalar rel_vel;
+			btVector3 vel;
+
+			int frictionIndex = m_tmpSolverContactConstraintPool.size();
+			btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
+			btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+			btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+			solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody();
+			solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody();
+			solverConstraint.m_originalContactPoint = &cp;
+
+			setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
+
+//			const btVector3& pos1 = cp.getPositionWorldOnA();
+//			const btVector3& pos2 = cp.getPositionWorldOnB();
+
+			/////setup the friction constraints
+
+			solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
+
+			if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+			{
+				cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+				btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+				if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
+				{
+					cp.m_lateralFrictionDir1 /= 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);
+					}
+
+					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);
+					cp.m_lateralFrictionInitialized = true;
+				} else
+				{
+					//re-calculate friction direction every frame, todo: check if this is really needed
+					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);
+					}
+
+					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);
+
+					cp.m_lateralFrictionInitialized = true;
 				}
-			}
-
-
-		}
-	}
-}
-
-
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+
+			} else
+			{
+				addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,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);
+			}
+			
+			setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal);
+
+		}
+	}
+}
+
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
 {
 	BT_PROFILE("solveGroupCacheFriendlySetup");
@@ -644,4 +746,31 @@
 	}
 
+	if (infoGlobal.m_splitImpulse)
+	{
+		for (int i = 0; i < numBodies; i++)
+		{
+			btRigidBody* body = btRigidBody::upcast(bodies[i]);
+			if (body)
+			{	
+				body->internalGetDeltaLinearVelocity().setZero();
+				body->internalGetDeltaAngularVelocity().setZero();
+				body->internalGetPushVelocity().setZero();
+				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();
+			}
+		}
+	}
+
 	if (1)
 	{
@@ -653,8 +782,4 @@
 		}
 	}
-
-	btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
-	initSolverBody(&fixedBody,0);
-
 	//btRigidBody* rb0=0,*rb1=0;
 
@@ -665,9 +790,10 @@
 			int totalNumRows = 0;
 			int i;
+			
+			m_tmpConstraintSizesPool.resize(numConstraints);
 			//calculate the total number of contraint rows
 			for (i=0;i<numConstraints;i++)
 			{
-
-				btTypedConstraint::btConstraintInfo1 info1;
+				btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
 				constraints[i]->getInfo1(&info1);
 				totalNumRows += info1.m_numConstraintRows;
@@ -675,14 +801,12 @@
 			m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
 
-			btTypedConstraint::btConstraintInfo1 info1;
-			info1.m_numConstraintRows = 0;
-
-
+			
 			///setup the btSolverConstraints
 			int currentRow = 0;
 
-			for (i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows)
+			for (i=0;i<numConstraints;i++)
 			{
-				constraints[i]->getInfo1(&info1);
+				const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+				
 				if (info1.m_numConstraintRows)
 				{
@@ -697,10 +821,5 @@
 					btRigidBody& rbB = constraint->getRigidBodyB();
 
-					int solverBodyIdA = getOrInitSolverBody(rbA);
-					int solverBodyIdB = getOrInitSolverBody(rbB);
-
-					btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
-					btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
-
+					
 					int j;
 					for ( j=0;j<info1.m_numConstraintRows;j++)
@@ -711,12 +830,12 @@
 						currentConstraintRow[j].m_appliedImpulse = 0.f;
 						currentConstraintRow[j].m_appliedPushImpulse = 0.f;
-						currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
-						currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
+						currentConstraintRow[j].m_solverBodyA = &rbA;
+						currentConstraintRow[j].m_solverBodyB = &rbB;
 					}
 
-					bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
-					bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
-					bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
-					bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
+					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);
 
 
@@ -733,7 +852,10 @@
 					btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
 					info2.m_constraintError = &currentConstraintRow->m_rhs;
+					currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
+					info2.m_damping = infoGlobal.m_damping;
 					info2.cfm = &currentConstraintRow->m_cfm;
 					info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
 					info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
+					info2.m_numIterations = infoGlobal.m_numIterations;
 					constraints[i]->getInfo2(&info2);
 
@@ -742,4 +864,5 @@
 					{
 						btSolverConstraint& solverConstraint = currentConstraintRow[j];
+						solverConstraint.m_originalContactPoint = constraint;
 
 						{
@@ -778,5 +901,5 @@
 							btScalar restitution = 0.f;
 							btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
-							btScalar	velocityError = restitution - rel_vel;// * damping;
+							btScalar	velocityError = restitution - rel_vel * info2.m_damping;
 							btScalar	penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
 							btScalar	velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
@@ -787,4 +910,5 @@
 					}
 				}
+				currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
 			}
 		}
@@ -793,5 +917,5 @@
 			int i;
 			btPersistentManifold* manifold = 0;
-			btCollisionObject* colObj0=0,*colObj1=0;
+//			btCollisionObject* colObj0=0,*colObj1=0;
 
 
@@ -830,11 +954,154 @@
 }
 
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
-{
-	BT_PROFILE("solveGroupCacheFriendlyIterations");
+btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
+{
 
 	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<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) {
+				int tmp = m_orderFrictionConstraintPool[j];
+				int swapi = btRandInt2(j+1);
+				m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
+				m_orderFrictionConstraintPool[swapi] = tmp;
+			}
+		}
+	}
+
+	if (infoGlobal.m_solverMode & SOLVER_SIMD)
+	{
+		///solve all joint constraints, using SIMD, if available
+		for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		{
+			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+			resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
+		}
+
+		for (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
+		int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+		for (j=0;j<numPoolConstraints;j++)
+		{
+			const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+			resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+
+		}
+		///solve all friction constraints, using SIMD, if available
+		int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+		for (j=0;j<numFrictionPoolConstraints;j++)
+		{
+			btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+			btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+			if (totalImpulse>btScalar(0))
+			{
+				solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+				solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+				resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA,	*solveManifold.m_solverBodyB,solveManifold);
+			}
+		}
+	} else
+	{
+
+		///solve all joint constraints
+		for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		{
+			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+			resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
+		}
+
+		for (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++)
+		{
+			const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+			resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+		}
+		///solve all friction constraints
+		int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+		for (j=0;j<numFrictionPoolConstraints;j++)
+		{
+			btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+			btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+			if (totalImpulse>btScalar(0))
+			{
+				solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+				solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+				resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+			}
+		}
+	}
+	return 0.f;
+}
+
+
+void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+{
+	int iteration;
+	if (infoGlobal.m_splitImpulse)
+	{
+		if (infoGlobal.m_solverMode & SOLVER_SIMD)
+		{
+			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+			{
+				{
+					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+					int j;
+					for (j=0;j<numPoolConstraints;j++)
+					{
+						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+
+						resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+					}
+				}
+			}
+		}
+		else
+		{
+			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+			{
+				{
+					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+					int j;
+					for (j=0;j<numPoolConstraints;j++)
+					{
+						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+
+						resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
+					}
+				}
+			}
+		}
+	}
+}
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+{
+	BT_PROFILE("solveGroupCacheFriendlyIterations");
+
+	
 	//should traverse the contacts random order...
 	int iteration;
@@ -842,138 +1109,16 @@
 		for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
 		{			
-
-			int j;
-			if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
-			{
-				if ((iteration & 7) == 0) {
-					for (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) {
-						int tmp = m_orderFrictionConstraintPool[j];
-						int swapi = btRandInt2(j+1);
-						m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
-						m_orderFrictionConstraintPool[swapi] = tmp;
-					}
-				}
-			}
-
-			if (infoGlobal.m_solverMode & SOLVER_SIMD)
-			{
-				///solve all joint constraints, using SIMD, if available
-				for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
-				{
-					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
-					resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
-				}
-
-				for (j=0;j<numConstraints;j++)
-				{
-					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
-					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
-					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
-					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
-					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
-				}
-
-				///solve all contact constraints using SIMD, if available
-				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-				for (j=0;j<numPoolConstraints;j++)
-				{
-					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-					resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-
-				}
-				///solve all friction constraints, using SIMD, if available
-				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-				for (j=0;j<numFrictionPoolConstraints;j++)
-				{
-					btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-					btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-
-					if (totalImpulse>btScalar(0))
-					{
-						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-
-						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],	m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-					}
-				}
-			} else
-			{
-
-				///solve all joint constraints
-				for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
-				{
-					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
-					resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
-				}
-
-				for (j=0;j<numConstraints;j++)
-				{
-					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
-					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
-					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
-					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
-
-					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
-				}
-
-				///solve all contact constraints
-				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-				for (j=0;j<numPoolConstraints;j++)
-				{
-					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-					resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-				}
-				///solve all friction constraints
-				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
-				for (j=0;j<numFrictionPoolConstraints;j++)
-				{
-					btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-					btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-
-					if (totalImpulse>btScalar(0))
-					{
-						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
-						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
-
-						resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],							m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-					}
-				}
-			}
-
-
-
-		}
+			solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
+		}
+		
+		solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
 	}
 	return 0.f;
 }
 
-
-
-/// btSequentialImpulseConstraintSolver Sequentially applies impulses
-btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
-{
-
-	
-
-	BT_PROFILE("solveGroup");
-	//we only implement SOLVER_CACHE_FRIENDLY now
-	//you need to provide at least some bodies
-	btAssert(bodies);
-	btAssert(numBodies);
-
-	int i;
-
-	solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
-	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
-
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** /*constraints*/,int /* numConstraints*/,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
+{
 	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
-	int j;
+	int i,j;
 
 	for (j=0;j<numPoolConstraints;j++)
@@ -993,20 +1138,34 @@
 	}
 
+	numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
+	for (j=0;j<numPoolConstraints;j++)
+	{
+		const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
+		btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
+		btScalar sum = constr->internalGetAppliedImpulse();
+		sum += solverConstr.m_appliedImpulse;
+		constr->internalSetAppliedImpulse(sum);
+	}
+
+
 	if (infoGlobal.m_splitImpulse)
 	{		
-		for ( i=0;i<m_tmpSolverBodyPool.size();i++)
-		{
-			m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
+		for ( i=0;i<numBodies;i++)
+		{
+			btRigidBody* body = btRigidBody::upcast(bodies[i]);
+			if (body)
+				body->internalWritebackVelocity(infoGlobal.m_timeStep);
 		}
 	} else
 	{
-		for ( i=0;i<m_tmpSolverBodyPool.size();i++)
-		{
-			m_tmpSolverBodyPool[i].writebackVelocity();
-		}
-	}
-
-
-	m_tmpSolverBodyPool.resize(0);
+		for ( i=0;i<numBodies;i++)
+		{
+			btRigidBody* body = btRigidBody::upcast(bodies[i]);
+			if (body)
+				body->internalWritebackVelocity();
+		}
+	}
+
+
 	m_tmpSolverContactConstraintPool.resize(0);
 	m_tmpSolverNonContactConstraintPool.resize(0);
@@ -1018,9 +1177,21 @@
 
 
-
-
-
-
-
+/// btSequentialImpulseConstraintSolver Sequentially applies impulses
+btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
+{
+
+	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);
+	
+	return 0.f;
+}
 
 void	btSequentialImpulseConstraintSolver::reset()
@@ -1029,3 +1200,9 @@
 }
 
-
+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;
+}
+
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h	(revision 8284)
@@ -22,6 +22,6 @@
 #include "btSolverBody.h"
 #include "btSolverConstraint.h"
-
-
+#include "btTypedConstraint.h"
+#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
 
 ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
@@ -30,5 +30,4 @@
 protected:
 
-	btAlignedObjectArray<btSolverBody>	m_tmpSolverBodyPool;
 	btConstraintArray			m_tmpSolverContactConstraintPool;
 	btConstraintArray			m_tmpSolverNonContactConstraintPool;
@@ -36,32 +35,61 @@
 	btAlignedObjectArray<int>	m_orderTmpConstraintPool;
 	btAlignedObjectArray<int>	m_orderFrictionConstraintPool;
+	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
 
-	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);
+	void setupFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* 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.);
 	
+	void setupContactConstraint(btSolverConstraint& solverConstraint, btCollisionObject* colObj0, btCollisionObject* colObj1, 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, 
+										 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);
+//	void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
 	btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
 
 	void	convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
 
+
+	void	resolveSplitPenetrationSIMD(
+        btRigidBody& body1,
+        btRigidBody& body2,
+        const btSolverConstraint& contactConstraint);
+
 	void	resolveSplitPenetrationImpulseCacheFriendly(
-        btSolverBody& body1,
-        btSolverBody& body2,
-        const btSolverConstraint& contactConstraint,
-        const btContactSolverInfo& solverInfo);
+        btRigidBody& body1,
+        btRigidBody& body2,
+        const btSolverConstraint& contactConstraint);
 
 	//internal method
 	int	getOrInitSolverBody(btCollisionObject& body);
 
-	void	resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+	void	resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
 
-	void	resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+	void	resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
 	
-	void	resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+	void	resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
 	
-	void	resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+	void	resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,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);
+	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);
+	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+
+
 public:
 
@@ -72,7 +100,6 @@
 	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
 	
-	btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
-	btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
 
+	
 	///clear internal cached data and reset random seed
 	virtual	void	reset();
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp	(revision 8284)
@@ -19,5 +19,5 @@
 */
 
-//-----------------------------------------------------------------------------
+
 
 #include "btSliderConstraint.h"
@@ -26,5 +26,5 @@
 #include <new>
 
-//-----------------------------------------------------------------------------
+#define USE_OFFSET_FOR_CONSTANT_FRAME true
 
 void btSliderConstraint::initParams()
@@ -37,19 +37,25 @@
 	m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
 	m_dampingDirLin = btScalar(0.);
+	m_cfmDirLin = SLIDER_CONSTRAINT_DEF_CFM;
 	m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
 	m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
 	m_dampingDirAng = btScalar(0.);
+	m_cfmDirAng = SLIDER_CONSTRAINT_DEF_CFM;
 	m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
 	m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
 	m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+	m_cfmOrthoLin = SLIDER_CONSTRAINT_DEF_CFM;
 	m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
 	m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
 	m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+	m_cfmOrthoAng = SLIDER_CONSTRAINT_DEF_CFM;
 	m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
 	m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
 	m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+	m_cfmLimLin = SLIDER_CONSTRAINT_DEF_CFM;
 	m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
 	m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
 	m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+	m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM;
 
 	m_poweredLinMotor = false;
@@ -63,62 +69,116 @@
 	m_accumulatedAngMotorImpulse = btScalar(0.0);
 
-} // btSliderConstraint::initParams()
-
-//-----------------------------------------------------------------------------
-
-btSliderConstraint::btSliderConstraint()
-        :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
-		m_useLinearReferenceFrameA(true),
-		m_useSolveConstraintObsolete(false)
-//		m_useSolveConstraintObsolete(true)
+	m_flags = 0;
+	m_flags = 0;
+
+	m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME;
+
+	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+
+
+
+
+
+btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
+        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB),
+		m_useSolveConstraintObsolete(false),
+		m_frameInA(frameInA),
+        m_frameInB(frameInB),
+		m_useLinearReferenceFrameA(useLinearReferenceFrameA)
 {
 	initParams();
-} // btSliderConstraint::btSliderConstraint()
-
-//-----------------------------------------------------------------------------
-
-btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
-        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB)
-        , m_frameInA(frameInA)
-        , m_frameInB(frameInB),
-		m_useLinearReferenceFrameA(useLinearReferenceFrameA),
-		m_useSolveConstraintObsolete(false)
-//		m_useSolveConstraintObsolete(true)
-{
+}
+
+
+
+btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA)
+        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB),
+		m_useSolveConstraintObsolete(false),
+		m_frameInB(frameInB),
+		m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+{
+	///not providing rigidbody A means implicitly using worldspace for body A
+	m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
+//	m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());
+
 	initParams();
-} // btSliderConstraint::btSliderConstraint()
-
-//-----------------------------------------------------------------------------
-
-void btSliderConstraint::buildJacobian()
-{
-	if (!m_useSolveConstraintObsolete) 
-	{
-		return;
-	}
-	if(m_useLinearReferenceFrameA)
-	{
-		buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB);
+}
+
+
+
+
+
+
+void btSliderConstraint::getInfo1(btConstraintInfo1* info)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
 	}
 	else
 	{
-		buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA);
-	}
-} // btSliderConstraint::buildJacobian()
-
-//-----------------------------------------------------------------------------
-
-void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
-{
-	//calculate transforms
-    m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA;
-    m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB;
+		info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
+		info->nub = 2; 
+		//prepare constraint
+		calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+		testAngLimits();
+		testLinLimits();
+		if(getSolveLinLimit() || getPoweredLinMotor())
+		{
+			info->m_numConstraintRows++; // limit 3rd linear as well
+			info->nub--; 
+		}
+		if(getSolveAngLimit() || getPoweredAngMotor())
+		{
+			info->m_numConstraintRows++; // limit 3rd angular as well
+			info->nub--; 
+		}
+	}
+}
+
+void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
+{
+
+	info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used)
+	info->nub = 0; 
+}
+
+void btSliderConstraint::getInfo2(btConstraintInfo2* info)
+{
+	getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
+}
+
+
+
+
+
+
+
+void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+{
+	if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
+	{
+		m_calculatedTransformA = transA * m_frameInA;
+		m_calculatedTransformB = transB * m_frameInB;
+	}
+	else
+	{
+		m_calculatedTransformA = transB * m_frameInB;
+		m_calculatedTransformB = transA * m_frameInA;
+	}
 	m_realPivotAInW = m_calculatedTransformA.getOrigin();
 	m_realPivotBInW = m_calculatedTransformB.getOrigin();
 	m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
-	m_delta = m_realPivotBInW - m_realPivotAInW;
+	if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
+	{
+		m_delta = m_realPivotBInW - m_realPivotAInW;
+	}
+	else
+	{
+		m_delta = m_realPivotAInW - m_realPivotBInW;
+	}
 	m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
-	m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition();
-	m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition();
     btVector3 normalWorld;
     int i;
@@ -127,79 +187,130 @@
     {
 		normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
-		new (&m_jacLin[i]) btJacobianEntry(
-			rbA.getCenterOfMassTransform().getBasis().transpose(),
-			rbB.getCenterOfMassTransform().getBasis().transpose(),
-			m_relPosA,
-			m_relPosB,
-			normalWorld,
-			rbA.getInvInertiaDiagLocal(),
-			rbA.getInvMass(),
-			rbB.getInvInertiaDiagLocal(),
-			rbB.getInvMass()
-			);
-		m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal();
 		m_depth[i] = m_delta.dot(normalWorld);
     }
-	testLinLimits();
-    // angular part
-    for(i = 0; i < 3; i++)
-    {
-		normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
-		new (&m_jacAng[i])	btJacobianEntry(
-			normalWorld,
-            rbA.getCenterOfMassTransform().getBasis().transpose(),
-            rbB.getCenterOfMassTransform().getBasis().transpose(),
-            rbA.getInvInertiaDiagLocal(),
-            rbB.getInvInertiaDiagLocal()
-			);
-	}
-	testAngLimits();
-	btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
-	m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA));
-	// clear accumulator for motors
-	m_accumulatedLinMotorImpulse = btScalar(0.0);
-	m_accumulatedAngMotorImpulse = btScalar(0.0);
-} // btSliderConstraint::buildJacobianInt()
-
-//-----------------------------------------------------------------------------
-
-void btSliderConstraint::getInfo1(btConstraintInfo1* info)
-{
-	if (m_useSolveConstraintObsolete)
-	{
-		info->m_numConstraintRows = 0;
-		info->nub = 0;
+}
+ 
+
+
+void btSliderConstraint::testLinLimits(void)
+{
+	m_solveLinLim = false;
+	m_linPos = m_depth[0];
+	if(m_lowerLinLimit <= m_upperLinLimit)
+	{
+		if(m_depth[0] > m_upperLinLimit)
+		{
+			m_depth[0] -= m_upperLinLimit;
+			m_solveLinLim = true;
+		}
+		else if(m_depth[0] < m_lowerLinLimit)
+		{
+			m_depth[0] -= m_lowerLinLimit;
+			m_solveLinLim = true;
+		}
+		else
+		{
+			m_depth[0] = btScalar(0.);
+		}
 	}
 	else
 	{
-		info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
-		info->nub = 2; 
-		//prepare constraint
-		calculateTransforms();
-		testLinLimits();
-		if(getSolveLinLimit() || getPoweredLinMotor())
-		{
-			info->m_numConstraintRows++; // limit 3rd linear as well
-			info->nub--; 
-		}
-		testAngLimits();
-		if(getSolveAngLimit() || getPoweredAngMotor())
-		{
-			info->m_numConstraintRows++; // limit 3rd angular as well
-			info->nub--; 
-		}
-	}
-} // btSliderConstraint::getInfo1()
-
-//-----------------------------------------------------------------------------
-
-void btSliderConstraint::getInfo2(btConstraintInfo2* info)
-{
+		m_depth[0] = btScalar(0.);
+	}
+}
+
+
+
+void btSliderConstraint::testAngLimits(void)
+{
+	m_angDepth = btScalar(0.);
+	m_solveAngLim = false;
+	if(m_lowerAngLimit <= m_upperAngLimit)
+	{
+		const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
+		const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
+		const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
+//		btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));  
+		btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0));  
+		rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit);
+		m_angPos = rot;
+		if(rot < m_lowerAngLimit)
+		{
+			m_angDepth = rot - m_lowerAngLimit;
+			m_solveAngLim = true;
+		} 
+		else if(rot > m_upperAngLimit)
+		{
+			m_angDepth = rot - m_upperAngLimit;
+			m_solveAngLim = true;
+		}
+	}
+}
+
+btVector3 btSliderConstraint::getAncorInA(void)
+{
+	btVector3 ancorInA;
+	ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
+	ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
+	return ancorInA;
+}
+
+
+
+btVector3 btSliderConstraint::getAncorInB(void)
+{
+	btVector3 ancorInB;
+	ancorInB = m_frameInB.getOrigin();
+	return ancorInB;
+}
+
+
+void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass  )
+{
+	const btTransform& trA = getCalculatedTransformA();
+	const btTransform& trB = getCalculatedTransformB();
+	
 	btAssert(!m_useSolveConstraintObsolete);
 	int i, s = info->rowskip;
-	const btTransform& trA = getCalculatedTransformA();
-	const btTransform& trB = getCalculatedTransformB();
+	
 	btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
-	// make rotations around Y and Z equal
+	
+	// difference between frames in WCS
+	btVector3 ofs = trB.getOrigin() - trA.getOrigin();
+	// now get weight factors depending on masses
+	btScalar miA = rbAinvMass;
+	btScalar miB = rbBinvMass;
+	bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
+	btScalar miS = miA + miB;
+	btScalar factA, factB;
+	if(miS > btScalar(0.f))
+	{
+		factA = miB / miS;
+	}
+	else 
+	{
+		factA = btScalar(0.5f);
+	}
+	factB = btScalar(1.0f) - factA;
+	btVector3 ax1, p, q;
+	btVector3 ax1A = trA.getBasis().getColumn(0);
+	btVector3 ax1B = trB.getBasis().getColumn(0);
+	if(m_useOffsetForConstraintFrame)
+	{
+		// get the desired direction of slider axis
+		// as weighted sum of X-orthos of frameA and frameB in WCS
+		ax1 = ax1A * factA + ax1B * factB;
+		ax1.normalize();
+		// construct two orthos to slider axis
+		btPlaneSpace1 (ax1, p, q);
+	}
+	else
+	{ // old way - use frameA
+		ax1 = trA.getBasis().getColumn(0);
+		// get 2 orthos to slider axis (Y, Z)
+		p = trA.getBasis().getColumn(1);
+		q = trA.getBasis().getColumn(2);
+	}
+	// make rotations around these orthos equal
 	// the slider axis should be the only unconstrained
 	// rotational axis, the angular velocity of the two bodies perpendicular to
@@ -209,10 +320,4 @@
 	// where p and q are unit vectors normal to the slider axis, and w1 and w2
 	// are the angular velocity vectors of the two bodies.
-	// get slider axis (X)
-	btVector3 ax1 = trA.getBasis().getColumn(0);
-	// get 2 orthos to slider axis (Y, Z)
-	btVector3 p = trA.getBasis().getColumn(1);
-	btVector3 q = trA.getBasis().getColumn(2);
-	// set the two slider rows 
 	info->m_J1angularAxis[0] = p[0];
 	info->m_J1angularAxis[1] = p[1];
@@ -230,6 +335,6 @@
 	// compute the right hand side of the constraint equation. set relative
 	// body velocities along p and q to bring the slider back into alignment.
-	// if ax1,ax2 are the unit length slider axes as computed from body1 and
-	// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
+	// if ax1A,ax1B are the unit length slider axes as computed from bodyA and
+	// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
 	// if "theta" is the angle between ax1 and ax2, we need an angular velocity
 	// along u to cover angle erp*theta in one step :
@@ -243,56 +348,118 @@
 	// ax1 x ax2 is in the plane space of ax1, so we project the angular
 	// velocity to p and q to find the right hand side.
-	btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
-    btVector3 ax2 = trB.getBasis().getColumn(0);
-	btVector3 u = ax1.cross(ax2);
+//	btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
+	btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
+	btScalar k = info->fps * currERP;
+
+	btVector3 u = ax1A.cross(ax1B);
 	info->m_constraintError[0] = k * u.dot(p);
 	info->m_constraintError[s] = k * u.dot(q);
-	// pull out pos and R for both bodies. also get the connection
-	// vector c = pos2-pos1.
-	// next two rows. we want: vel2 = vel1 + w1 x c ... but this would
-	// result in three equations, so we project along the planespace vectors
-	// so that sliding along the slider axis is disregarded. for symmetry we
-	// also consider rotation around center of mass of two bodies (factA and factB).
-	btTransform bodyA_trans = m_rbA.getCenterOfMassTransform();
-	btTransform bodyB_trans = m_rbB.getCenterOfMassTransform();
-	int s2 = 2 * s, s3 = 3 * s;
-	btVector3 c;
-	btScalar miA = m_rbA.getInvMass();
-	btScalar miB = m_rbB.getInvMass();
-	btScalar miS = miA + miB;
-	btScalar factA, factB;
-	if(miS > btScalar(0.f))
-	{
-		factA = miB / miS;
-	}
-	else 
-	{
-		factA = btScalar(0.5f);
-	}
-	if(factA > 0.99f) factA = 0.99f;
-	if(factA < 0.01f) factA = 0.01f;
-	factB = btScalar(1.0f) - factA;
-	c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
-	btVector3 tmp = c.cross(p);
-	for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
-	for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
-	tmp = c.cross(q);
-	for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
-	for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
-
-	for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
-	for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
-	// compute two elements of right hand side. we want to align the offset
-	// point (in body 2's frame) with the center of body 1.
-	btVector3 ofs; // offset point in global coordinates
-	ofs = trB.getOrigin() - trA.getOrigin();
-	k = info->fps * info->erp * getSoftnessOrthoLin();
-	info->m_constraintError[s2] = k * p.dot(ofs);
-	info->m_constraintError[s3] = k * q.dot(ofs);
-	int nrow = 3; // last filled row
+	if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
+	{
+		info->cfm[0] = m_cfmOrthoAng;
+		info->cfm[s] = m_cfmOrthoAng;
+	}
+
+	int nrow = 1; // last filled row
 	int srow;
-	// check linear limits linear
-	btScalar limit_err = btScalar(0.0);
-	int limit = 0;
+	btScalar limit_err;
+	int limit;
+	int powered;
+
+	// next two rows. 
+	// we want: velA + wA x relA == velB + wB x relB ... but this would
+	// result in three equations, so we project along two orthos to the slider axis
+
+	btTransform bodyA_trans = transA;
+	btTransform bodyB_trans = transB;
+	nrow++;
+	int s2 = nrow * s;
+	nrow++;
+	int s3 = nrow * s;
+	btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0);
+	if(m_useOffsetForConstraintFrame)
+	{
+		// get vector from bodyB to frameB in WCS
+		relB = trB.getOrigin() - bodyB_trans.getOrigin();
+		// get its projection to slider axis
+		btVector3 projB = ax1 * relB.dot(ax1);
+		// get vector directed from bodyB to slider axis (and orthogonal to it)
+		btVector3 orthoB = relB - projB;
+		// same for bodyA
+		relA = trA.getOrigin() - bodyA_trans.getOrigin();
+		btVector3 projA = ax1 * relA.dot(ax1);
+		btVector3 orthoA = relA - projA;
+		// get desired offset between frames A and B along slider axis
+		btScalar sliderOffs = m_linPos - m_depth[0];
+		// desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
+		btVector3 totalDist = projA + ax1 * sliderOffs - projB;
+		// get offset vectors relA and relB
+		relA = orthoA + totalDist * factA;
+		relB = orthoB - totalDist * factB;
+		// now choose average ortho to slider axis
+		p = orthoB * factA + orthoA * factB;
+		btScalar len2 = p.length2();
+		if(len2 > SIMD_EPSILON)
+		{
+			p /= btSqrt(len2);
+		}
+		else
+		{
+			p = trA.getBasis().getColumn(1);
+		}
+		// make one more ortho
+		q = ax1.cross(p);
+		// fill two rows
+		tmpA = relA.cross(p);
+		tmpB = relB.cross(p);
+		for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
+		for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
+		tmpA = relA.cross(q);
+		tmpB = relB.cross(q);
+		if(hasStaticBody && getSolveAngLimit())
+		{ // to make constraint between static and dynamic objects more rigid
+			// remove wA (or wB) from equation if angular limit is hit
+			tmpB *= factB;
+			tmpA *= factA;
+		}
+		for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i];
+		for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i];
+		for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
+		for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
+	}
+	else
+	{	// old way - maybe incorrect if bodies are not on the slider axis
+		// see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
+		c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
+		btVector3 tmp = c.cross(p);
+		for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
+		for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
+		tmp = c.cross(q);
+		for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
+		for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
+
+		for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
+		for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
+	}
+	// compute two elements of right hand side
+
+	//	k = info->fps * info->erp * getSoftnessOrthoLin();
+	currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp;
+	k = info->fps * currERP;
+
+	btScalar rhs = k * p.dot(ofs);
+	info->m_constraintError[s2] = rhs;
+	rhs = k * q.dot(ofs);
+	info->m_constraintError[s3] = rhs;
+	if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
+	{
+		info->cfm[s2] = m_cfmOrthoLin;
+		info->cfm[s3] = m_cfmOrthoLin;
+	}
+
+
+	// check linear limits
+	limit_err = btScalar(0.0);
+	limit = 0;
 	if(getSolveLinLimit())
 	{
@@ -300,5 +467,5 @@
 		limit = (limit_err > btScalar(0.0)) ? 2 : 1;
 	}
-	int powered = 0;
+	powered = 0;
 	if(getPoweredLinMotor())
 	{
@@ -320,14 +487,30 @@
 		// a torque couple will result in limited slider-jointed free
 		// bodies from gaining angular momentum.
-		// the solution used here is to apply the constraint forces at the center of mass of the two bodies
-		btVector3 ltd;	// Linear Torque Decoupling vector (a torque)
-//		c = btScalar(0.5) * c;
-		ltd = c.cross(ax1);
-		info->m_J1angularAxis[srow+0] = factA*ltd[0];
-		info->m_J1angularAxis[srow+1] = factA*ltd[1];
-		info->m_J1angularAxis[srow+2] = factA*ltd[2];
-		info->m_J2angularAxis[srow+0] = factB*ltd[0];
-		info->m_J2angularAxis[srow+1] = factB*ltd[1];
-		info->m_J2angularAxis[srow+2] = factB*ltd[2];
+		if(m_useOffsetForConstraintFrame)
+		{
+			// this is needed only when bodyA and bodyB are both dynamic.
+			if(!hasStaticBody)
+			{
+				tmpA = relA.cross(ax1);
+				tmpB = relB.cross(ax1);
+				info->m_J1angularAxis[srow+0] = tmpA[0];
+				info->m_J1angularAxis[srow+1] = tmpA[1];
+				info->m_J1angularAxis[srow+2] = tmpA[2];
+				info->m_J2angularAxis[srow+0] = -tmpB[0];
+				info->m_J2angularAxis[srow+1] = -tmpB[1];
+				info->m_J2angularAxis[srow+2] = -tmpB[2];
+			}
+		}
+		else
+		{ // The old way. May be incorrect if bodies are not on the slider axis
+			btVector3 ltd;	// Linear Torque Decoupling vector (a torque)
+			ltd = c.cross(ax1);
+			info->m_J1angularAxis[srow+0] = factA*ltd[0];
+			info->m_J1angularAxis[srow+1] = factA*ltd[1];
+			info->m_J1angularAxis[srow+2] = factA*ltd[2];
+			info->m_J2angularAxis[srow+0] = factB*ltd[0];
+			info->m_J2angularAxis[srow+1] = factB*ltd[1];
+			info->m_J2angularAxis[srow+2] = factB*ltd[2];
+		}
 		// right-hand part
 		btScalar lostop = getLowerLinLimit();
@@ -340,10 +523,13 @@
 		info->m_lowerLimit[srow] = 0.;
 		info->m_upperLimit[srow] = 0.;
+		currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp;
 		if(powered)
 		{
-            info->cfm[nrow] = btScalar(0.0); 
+			if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
+			{
+				info->cfm[srow] = m_cfmDirLin;
+			}
 			btScalar tag_vel = getTargetLinMotorVelocity();
-			btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp);
-//			info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity();
+			btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
 			info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
 			info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
@@ -352,7 +538,10 @@
 		if(limit)
 		{
-			k = info->fps * info->erp;
+			k = info->fps * currERP;
 			info->m_constraintError[srow] += k * limit_err;
-			info->cfm[srow] = btScalar(0.0); // stop_cfm;
+			if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
+			{
+				info->cfm[srow] = m_cfmLimLin;
+			}
 			if(lostop == histop) 
 			{	// limited low and high simultaneously
@@ -374,6 +563,6 @@
 			if(bounce > btScalar(0.0))
 			{
-				btScalar vel = m_rbA.getLinearVelocity().dot(ax1);
-				vel -= m_rbB.getLinearVelocity().dot(ax1);
+				btScalar vel = linVelA.dot(ax1);
+				vel -= linVelB.dot(ax1);
 				vel *= signFact;
 				// only apply bounce if the velocity is incoming, and if the
@@ -437,8 +626,12 @@
 			powered = 0;
 		}
+		currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp;
 		if(powered)
 		{
-            info->cfm[srow] = btScalar(0.0); 
-			btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
+			if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
+			{
+				info->cfm[srow] = m_cfmDirAng;
+			}
+			btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
 			info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
 			info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
@@ -447,7 +640,10 @@
 		if(limit)
 		{
-			k = info->fps * info->erp;
+			k = info->fps * currERP;
 			info->m_constraintError[srow] += k * limit_err;
-			info->cfm[srow] = btScalar(0.0); // stop_cfm;
+			if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
+			{
+				info->cfm[srow] = m_cfmLimAng;
+			}
 			if(lostop == histop) 
 			{
@@ -500,318 +696,162 @@
 		} // if(limit)
 	} // if angular limit or powered
-} // btSliderConstraint::getInfo2()
-
-//-----------------------------------------------------------------------------
-
-void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
-{
-	if (m_useSolveConstraintObsolete)
-	{
-		m_timeStep = timeStep;
-		if(m_useLinearReferenceFrameA)
-		{
-			solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB);
+}
+
+
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+///If no axis is provided, it uses the default axis for this constraint.
+void btSliderConstraint::setParam(int num, btScalar value, int axis)
+{
+	switch(num)
+	{
+	case BT_CONSTRAINT_STOP_ERP :
+		if(axis < 1)
+		{
+			m_softnessLimLin = value;
+			m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN;
+		}
+		else if(axis < 3)
+		{
+			m_softnessOrthoLin = value;
+			m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN;
+		}
+		else if(axis == 3)
+		{
+			m_softnessLimAng = value;
+			m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG;
+		}
+		else if(axis < 6)
+		{
+			m_softnessOrthoAng = value;
+			m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG;
 		}
 		else
 		{
-			solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
-		}
-	}
-} // btSliderConstraint::solveConstraint()
-
-//-----------------------------------------------------------------------------
-
-void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
-{
-    int i;
-    // linear
-    btVector3 velA;
-	bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
-    btVector3 velB;
-	bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
-    btVector3 vel = velA - velB;
-	for(i = 0; i < 3; i++)
-    {
-		const btVector3& normal = m_jacLin[i].m_linearJointAxis;
-		btScalar rel_vel = normal.dot(vel);
-		// calculate positional error
-		btScalar depth = m_depth[i];
-		// get parameters
-		btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
-		btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
-		btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
-		// calcutate and apply impulse
-		btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
-		btVector3 impulse_vector = normal * normalImpulse;
-		
-		//rbA.applyImpulse( impulse_vector, m_relPosA);
-		//rbB.applyImpulse(-impulse_vector, m_relPosB);
-		{
-			btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
-			btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
-			bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
-			bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
-		}
-
-
-
-		if(m_poweredLinMotor && (!i))
-		{ // apply linear motor
-			if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
-			{
-				btScalar desiredMotorVel = m_targetLinMotorVelocity;
-				btScalar motor_relvel = desiredMotorVel + rel_vel;
-				normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
-				// clamp accumulated impulse
-				btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse);
-				if(new_acc  > m_maxLinMotorForce)
-				{
-					new_acc = m_maxLinMotorForce;
-				}
-				btScalar del = new_acc  - m_accumulatedLinMotorImpulse;
-				if(normalImpulse < btScalar(0.0))
-				{
-					normalImpulse = -del;
-				}
-				else
-				{
-					normalImpulse = del;
-				}
-				m_accumulatedLinMotorImpulse = new_acc;
-				// apply clamped impulse
-				impulse_vector = normal * normalImpulse;
-				//rbA.applyImpulse( impulse_vector, m_relPosA);
-				//rbB.applyImpulse(-impulse_vector, m_relPosB);
-
-				{
-					btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
-					btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
-					bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
-					bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
-				}
-
-
-
-			}
-		}
-    }
-	// angular 
-	// get axes in world space
-	btVector3 axisA =  m_calculatedTransformA.getBasis().getColumn(0);
-	btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);
-
-	btVector3 angVelA;
-	bodyA.getAngularVelocity(angVelA);
-	btVector3 angVelB;
-	bodyB.getAngularVelocity(angVelB);
-
-	btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
-	btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
-
-	btVector3 angAorthog = angVelA - angVelAroundAxisA;
-	btVector3 angBorthog = angVelB - angVelAroundAxisB;
-	btVector3 velrelOrthog = angAorthog-angBorthog;
-	//solve orthogonal angular velocity correction
-	btScalar len = velrelOrthog.length();
-	btScalar orthorImpulseMag = 0.f;
-
-	if (len > btScalar(0.00001))
-	{
-		btVector3 normal = velrelOrthog.normalized();
-		btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
-		//velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
-		orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
-	}
-	//solve angular positional correction
-	btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
-	btVector3 angularAxis = angularError;
-	btScalar angularImpulseMag = 0;
-
-	btScalar len2 = angularError.length();
-	if (len2>btScalar(0.00001))
-	{
-		btVector3 normal2 = angularError.normalized();
-		btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
-		angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
-		angularError *= angularImpulseMag;
-	}
-	// apply impulse
-	//rbA.applyTorqueImpulse(-velrelOrthog+angularError);
-	//rbB.applyTorqueImpulse(velrelOrthog-angularError);
-
-	bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag);
-	bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag);
-	bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag);
-	bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag);
-
-
-	btScalar impulseMag;
-	//solve angular limits
-	if(m_solveAngLim)
-	{
-		impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep;
-		impulseMag *= m_kAngle * m_softnessLimAng;
-	}
-	else
-	{
-		impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep;
-		impulseMag *= m_kAngle * m_softnessDirAng;
-	}
-	btVector3 impulse = axisA * impulseMag;
-	//rbA.applyTorqueImpulse(impulse);
-	//rbB.applyTorqueImpulse(-impulse);
-
-	bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag);
-	bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag);
-
-
-
-	//apply angular motor
-	if(m_poweredAngMotor) 
-	{
-		if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce)
-		{
-			btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
-			btScalar projRelVel = velrel.dot(axisA);
-
-			btScalar desiredMotorVel = m_targetAngMotorVelocity;
-			btScalar motor_relvel = desiredMotorVel - projRelVel;
-
-			btScalar angImpulse = m_kAngle * motor_relvel;
-			// clamp accumulated impulse
-			btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse);
-			if(new_acc  > m_maxAngMotorForce)
-			{
-				new_acc = m_maxAngMotorForce;
-			}
-			btScalar del = new_acc  - m_accumulatedAngMotorImpulse;
-			if(angImpulse < btScalar(0.0))
-			{
-				angImpulse = -del;
-			}
-			else
-			{
-				angImpulse = del;
-			}
-			m_accumulatedAngMotorImpulse = new_acc;
-			// apply clamped impulse
-			btVector3 motorImp = angImpulse * axisA;
-			//rbA.applyTorqueImpulse(motorImp);
-			//rbB.applyTorqueImpulse(-motorImp);
-
-			bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse);
-			bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse);
-		}
-	}
-} // btSliderConstraint::solveConstraint()
-
-//-----------------------------------------------------------------------------
-
-//-----------------------------------------------------------------------------
-
-void btSliderConstraint::calculateTransforms(void){
-	if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
-	{
-		m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
-		m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
-	}
-	else
-	{
-		m_calculatedTransformA = m_rbB.getCenterOfMassTransform() * m_frameInB;
-		m_calculatedTransformB = m_rbA.getCenterOfMassTransform() * m_frameInA;
-	}
-	m_realPivotAInW = m_calculatedTransformA.getOrigin();
-	m_realPivotBInW = m_calculatedTransformB.getOrigin();
-	m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
-	if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
-	{
-		m_delta = m_realPivotBInW - m_realPivotAInW;
-	}
-	else
-	{
-		m_delta = m_realPivotAInW - m_realPivotBInW;
-	}
-	m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
-    btVector3 normalWorld;
-    int i;
-    //linear part
-    for(i = 0; i < 3; i++)
-    {
-		normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
-		m_depth[i] = m_delta.dot(normalWorld);
-    }
-} // btSliderConstraint::calculateTransforms()
- 
-//-----------------------------------------------------------------------------
-
-void btSliderConstraint::testLinLimits(void)
-{
-	m_solveLinLim = false;
-	m_linPos = m_depth[0];
-	if(m_lowerLinLimit <= m_upperLinLimit)
-	{
-		if(m_depth[0] > m_upperLinLimit)
-		{
-			m_depth[0] -= m_upperLinLimit;
-			m_solveLinLim = true;
-		}
-		else if(m_depth[0] < m_lowerLinLimit)
-		{
-			m_depth[0] -= m_lowerLinLimit;
-			m_solveLinLim = true;
+			btAssertConstrParams(0);
+		}
+		break;
+	case BT_CONSTRAINT_CFM :
+		if(axis < 1)
+		{
+			m_cfmDirLin = value;
+			m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN;
+		}
+		else if(axis == 3)
+		{
+			m_cfmDirAng = value;
+			m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG;
 		}
 		else
 		{
-			m_depth[0] = btScalar(0.);
-		}
-	}
-	else
-	{
-		m_depth[0] = btScalar(0.);
-	}
-} // btSliderConstraint::testLinLimits()
-
-//-----------------------------------------------------------------------------
-
-void btSliderConstraint::testAngLimits(void)
-{
-	m_angDepth = btScalar(0.);
-	m_solveAngLim = false;
-	if(m_lowerAngLimit <= m_upperAngLimit)
-	{
-		const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
-		const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
-		const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
-		btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));  
-		m_angPos = rot;
-		if(rot < m_lowerAngLimit)
-		{
-			m_angDepth = rot - m_lowerAngLimit;
-			m_solveAngLim = true;
-		} 
-		else if(rot > m_upperAngLimit)
-		{
-			m_angDepth = rot - m_upperAngLimit;
-			m_solveAngLim = true;
-		}
-	}
-} // btSliderConstraint::testAngLimits()
-	
-//-----------------------------------------------------------------------------
-
-btVector3 btSliderConstraint::getAncorInA(void)
-{
-	btVector3 ancorInA;
-	ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
-	ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
-	return ancorInA;
-} // btSliderConstraint::getAncorInA()
-
-//-----------------------------------------------------------------------------
-
-btVector3 btSliderConstraint::getAncorInB(void)
-{
-	btVector3 ancorInB;
-	ancorInB = m_frameInB.getOrigin();
-	return ancorInB;
-} // btSliderConstraint::getAncorInB();
+			btAssertConstrParams(0);
+		}
+		break;
+	case BT_CONSTRAINT_STOP_CFM :
+		if(axis < 1)
+		{
+			m_cfmLimLin = value;
+			m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN;
+		}
+		else if(axis < 3)
+		{
+			m_cfmOrthoLin = value;
+			m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN;
+		}
+		else if(axis == 3)
+		{
+			m_cfmLimAng = value;
+			m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG;
+		}
+		else if(axis < 6)
+		{
+			m_cfmOrthoAng = value;
+			m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG;
+		}
+		else
+		{
+			btAssertConstrParams(0);
+		}
+		break;
+	}
+}
+
+///return the local value of parameter
+btScalar btSliderConstraint::getParam(int num, int axis) const 
+{
+	btScalar retVal(SIMD_INFINITY);
+	switch(num)
+	{
+	case BT_CONSTRAINT_STOP_ERP :
+		if(axis < 1)
+		{
+			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN);
+			retVal = m_softnessLimLin;
+		}
+		else if(axis < 3)
+		{
+			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN);
+			retVal = m_softnessOrthoLin;
+		}
+		else if(axis == 3)
+		{
+			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG);
+			retVal = m_softnessLimAng;
+		}
+		else if(axis < 6)
+		{
+			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG);
+			retVal = m_softnessOrthoAng;
+		}
+		else
+		{
+			btAssertConstrParams(0);
+		}
+		break;
+	case BT_CONSTRAINT_CFM :
+		if(axis < 1)
+		{
+			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN);
+			retVal = m_cfmDirLin;
+		}
+		else if(axis == 3)
+		{
+			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG);
+			retVal = m_cfmDirAng;
+		}
+		else
+		{
+			btAssertConstrParams(0);
+		}
+		break;
+	case BT_CONSTRAINT_STOP_CFM :
+		if(axis < 1)
+		{
+			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN);
+			retVal = m_cfmLimLin;
+		}
+		else if(axis < 3)
+		{
+			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN);
+			retVal = m_cfmOrthoLin;
+		}
+		else if(axis == 3)
+		{
+			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG);
+			retVal = m_cfmLimAng;
+		}
+		else if(axis < 6)
+		{
+			btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG);
+			retVal = m_cfmOrthoAng;
+		}
+		else
+		{
+			btAssertConstrParams(0);
+		}
+		break;
+	}
+	return retVal;
+}
+
+
+
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h	(revision 8284)
@@ -26,5 +26,5 @@
 #define SLIDER_CONSTRAINT_H
 
-//-----------------------------------------------------------------------------
+
 
 #include "LinearMath/btVector3.h"
@@ -32,15 +32,32 @@
 #include "btTypedConstraint.h"
 
-//-----------------------------------------------------------------------------
+
 
 class btRigidBody;
 
-//-----------------------------------------------------------------------------
+
 
 #define SLIDER_CONSTRAINT_DEF_SOFTNESS		(btScalar(1.0))
 #define SLIDER_CONSTRAINT_DEF_DAMPING		(btScalar(1.0))
 #define SLIDER_CONSTRAINT_DEF_RESTITUTION	(btScalar(0.7))
-
-//-----------------------------------------------------------------------------
+#define SLIDER_CONSTRAINT_DEF_CFM			(btScalar(0.f))
+
+
+enum btSliderFlags
+{
+	BT_SLIDER_FLAGS_CFM_DIRLIN = (1 << 0),
+	BT_SLIDER_FLAGS_ERP_DIRLIN = (1 << 1),
+	BT_SLIDER_FLAGS_CFM_DIRANG = (1 << 2),
+	BT_SLIDER_FLAGS_ERP_DIRANG = (1 << 3),
+	BT_SLIDER_FLAGS_CFM_ORTLIN = (1 << 4),
+	BT_SLIDER_FLAGS_ERP_ORTLIN = (1 << 5),
+	BT_SLIDER_FLAGS_CFM_ORTANG = (1 << 6),
+	BT_SLIDER_FLAGS_ERP_ORTANG = (1 << 7),
+	BT_SLIDER_FLAGS_CFM_LIMLIN = (1 << 8),
+	BT_SLIDER_FLAGS_ERP_LIMLIN = (1 << 9),
+	BT_SLIDER_FLAGS_CFM_LIMANG = (1 << 10),
+	BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11)
+};
+
 
 class btSliderConstraint : public btTypedConstraint
@@ -49,4 +66,5 @@
 	///for backwards compatibility during the transition to 'getInfo/getInfo2'
 	bool		m_useSolveConstraintObsolete;
+	bool		m_useOffsetForConstraintFrame;
 	btTransform	m_frameInA;
     btTransform	m_frameInB;
@@ -68,23 +86,36 @@
 	btScalar m_restitutionDirLin;
 	btScalar m_dampingDirLin;
+	btScalar m_cfmDirLin;
+
 	btScalar m_softnessDirAng;
 	btScalar m_restitutionDirAng;
 	btScalar m_dampingDirAng;
+	btScalar m_cfmDirAng;
+
 	btScalar m_softnessLimLin;
 	btScalar m_restitutionLimLin;
 	btScalar m_dampingLimLin;
+	btScalar m_cfmLimLin;
+
 	btScalar m_softnessLimAng;
 	btScalar m_restitutionLimAng;
 	btScalar m_dampingLimAng;
+	btScalar m_cfmLimAng;
+
 	btScalar m_softnessOrthoLin;
 	btScalar m_restitutionOrthoLin;
 	btScalar m_dampingOrthoLin;
+	btScalar m_cfmOrthoLin;
+
 	btScalar m_softnessOrthoAng;
 	btScalar m_restitutionOrthoAng;
 	btScalar m_dampingOrthoAng;
+	btScalar m_cfmOrthoAng;
 	
 	// for interlal use
 	bool m_solveLinLim;
 	bool m_solveAngLim;
+
+	int m_flags;
 
 	btJacobianEntry	m_jacLin[3];
@@ -127,13 +158,16 @@
 	// constructors
     btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
-    btSliderConstraint();
+    btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
+
 	// overrides
-    virtual void	buildJacobian();
+
     virtual void getInfo1 (btConstraintInfo1* info);
+
+	void getInfo1NonVirtual(btConstraintInfo1* info);
 	
 	virtual void getInfo2 (btConstraintInfo2* info);
 
-    virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
-	
+	void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass);
+
 
 	// access
@@ -151,7 +185,7 @@
     void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
     btScalar getLowerAngLimit() { return m_lowerAngLimit; }
-    void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; }
+    void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
     btScalar getUpperAngLimit() { return m_upperAngLimit; }
-    void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; }
+    void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
 	bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
 	btScalar getSoftnessDirLin() { return m_softnessDirLin; }
@@ -211,18 +245,76 @@
 	bool getSolveAngLimit() { return m_solveAngLim; }
 	btScalar getAngDepth() { return m_angDepth; }
-	// internal
-    void	buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
-    void	solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
 	// shared code used by ODE solver
-	void	calculateTransforms(void);
-	void	testLinLimits(void);
-	void	testLinLimits2(btConstraintInfo2* info);
-	void	testAngLimits(void);
+	void	calculateTransforms(const btTransform& transA,const btTransform& transB);
+	void	testLinLimits();
+	void	testAngLimits();
 	// access for PE Solver
-	btVector3 getAncorInA(void);
-	btVector3 getAncorInB(void);
+	btVector3 getAncorInA();
+	btVector3 getAncorInB();
+	// access for UseFrameOffset
+	bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
+	void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
+
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///If no axis is provided, it uses the default axis for this constraint.
+	virtual	void	setParam(int num, btScalar value, int axis = -1);
+	///return the local value of parameter
+	virtual	btScalar getParam(int num, int axis = -1) const;
+
+	virtual	int	calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+
+
 };
 
-//-----------------------------------------------------------------------------
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btSliderConstraintData
+{
+	btTypedConstraintData	m_typeConstraintData;
+	btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTransformFloatData m_rbBFrame;
+	
+	float	m_linearUpperLimit;
+	float	m_linearLowerLimit;
+
+	float	m_angularUpperLimit;
+	float	m_angularLowerLimit;
+
+	int	m_useLinearReferenceFrameA;
+	int m_useOffsetForConstraintFrame;
+
+};
+
+
+SIMD_FORCE_INLINE		int	btSliderConstraint::calculateSerializeBufferSize() const
+{
+	return sizeof(btSliderConstraintData);
+}
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE	const char*	btSliderConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+
+	btSliderConstraintData* sliderData = (btSliderConstraintData*) dataBuffer;
+	btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer);
+
+	m_frameInA.serializeFloat(sliderData->m_rbAFrame);
+	m_frameInB.serializeFloat(sliderData->m_rbBFrame);
+
+	sliderData->m_linearUpperLimit = float(m_upperLinLimit);
+	sliderData->m_linearLowerLimit = float(m_lowerLinLimit);
+
+	sliderData->m_angularUpperLimit = float(m_upperAngLimit);
+	sliderData->m_angularLowerLimit = float(m_lowerAngLimit);
+
+	sliderData->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA;
+	sliderData->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame;
+
+	return "btSliderConstraintData";
+}
+
+
 
 #endif //SLIDER_CONSTRAINT_H
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h	(revision 8284)
@@ -106,15 +106,14 @@
 
 ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
-ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
+ATTRIBUTE_ALIGNED64 (struct)	btSolverBodyObsolete
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 	btVector3		m_deltaLinearVelocity;
 	btVector3		m_deltaAngularVelocity;
-	btScalar		m_angularFactor;
-	btScalar		m_invMass;
-	btScalar		m_friction;
+	btVector3		m_angularFactor;
+	btVector3		m_invMass;
 	btRigidBody*	m_originalBody;
 	btVector3		m_pushVelocity;
-	//btVector3		m_turnVelocity;	
+	btVector3		m_turnVelocity;
 
 	
@@ -146,10 +145,16 @@
 	}
 
-	
-/*
+	SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	{
+		if (m_originalBody)
+		{
+			m_pushVelocity += linearComponent*impulseMagnitude;
+			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		}
+	}
 	
 	void	writebackVelocity()
 	{
-		if (m_invMass)
+		if (m_originalBody)
 		{
 			m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
@@ -159,12 +164,19 @@
 		}
 	}
-	*/
 
-	void	writebackVelocity(btScalar timeStep=0)
+
+	void	writebackVelocity(btScalar timeStep)
 	{
-		if (m_invMass)
+        (void) timeStep;
+		if (m_originalBody)
 		{
-			m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
+			m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
 			m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
+			
+			//correct the position/orientation based on push/turn recovery
+			btTransform newTransform;
+			btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
+			m_originalBody->setWorldTransform(newTransform);
+			
 			//m_originalBody->setCompanionId(-1);
 		}
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h	(revision 8284)
@@ -27,5 +27,5 @@
 
 ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
-ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
+ATTRIBUTE_ALIGNED64 (struct)	btSolverConstraint
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -59,11 +59,11 @@
 	union
 	{
-		int			m_solverBodyIdA;
-		btScalar	m_unusedPadding2;
+		btRigidBody*	m_solverBodyA;
+		int				m_companionIdA;
 	};
 	union
 	{
-		int			m_solverBodyIdB;
-		btScalar	m_unusedPadding3;
+		btRigidBody*	m_solverBodyB;
+		int				m_companionIdB;
 	};
 	
@@ -78,4 +78,6 @@
 	btScalar		m_lowerLimit;
 	btScalar		m_upperLimit;
+
+	btScalar		m_rhsPenetration;
 
 	enum		btSolverConstraintType
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp	(revision 8284)
@@ -17,38 +17,27 @@
 #include "btTypedConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btSerializer.h"
 
-static btRigidBody s_fixed(0, 0,0);
 
 #define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
 
-btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
-:m_userConstraintType(-1),
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
+:btTypedObject(type),
+m_userConstraintType(-1),
 m_userConstraintId(-1),
-m_constraintType (type),
-m_rbA(s_fixed),
-m_rbB(s_fixed),
+m_needsFeedback(false),
+m_rbA(rbA),
+m_rbB(getFixedBody()),
 m_appliedImpulse(btScalar(0.)),
 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
 {
-	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
-}
-btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
-:m_userConstraintType(-1),
-m_userConstraintId(-1),
-m_constraintType (type),
-m_rbA(rbA),
-m_rbB(s_fixed),
-m_appliedImpulse(btScalar(0.)),
-m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
-{
-		s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
-
 }
 
 
 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
-:m_userConstraintType(-1),
+:btTypedObject(type),
+m_userConstraintType(-1),
 m_userConstraintId(-1),
-m_constraintType (type),
+m_needsFeedback(false),
 m_rbA(rbA),
 m_rbB(rbB),
@@ -56,10 +45,8 @@
 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
 {
-		s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
-
 }
 
 
-//-----------------------------------------------------------------------------
+
 
 btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
@@ -110,5 +97,46 @@
 	}
 	return lim_fact;
-} // btTypedConstraint::getMotorFactor()
+}
 
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*	btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btTypedConstraintData* tcd = (btTypedConstraintData*) dataBuffer;
 
+	tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
+	tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
+	char* name = (char*) serializer->findNameForPointer(this);
+	tcd->m_name = (char*)serializer->getUniquePointer(name);
+	if (tcd->m_name)
+	{
+		serializer->serializeName(name);
+	}
+
+	tcd->m_objectType = m_objectType;
+	tcd->m_needsFeedback = m_needsFeedback;
+	tcd->m_userConstraintId =m_userConstraintId;
+	tcd->m_userConstraintType =m_userConstraintType;
+
+	tcd->m_appliedImpulse = float(m_appliedImpulse);
+	tcd->m_dbgDrawSize = float(m_dbgDrawSize );
+
+	tcd->m_disableCollisionsBetweenLinkedBodies = false;
+
+	int i;
+	for (i=0;i<m_rbA.getNumConstraintRefs();i++)
+		if (m_rbA.getConstraintRef(i) == this)
+			tcd->m_disableCollisionsBetweenLinkedBodies = true;
+	for (i=0;i<m_rbB.getNumConstraintRefs();i++)
+		if (m_rbB.getConstraintRef(i) == this)
+			tcd->m_disableCollisionsBetweenLinkedBodies = true;
+
+	return "btTypedConstraintData";
+}
+
+btRigidBody& btTypedConstraint::getFixedBody()
+{
+	static btRigidBody s_fixed(0, 0,0);
+	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+	return s_fixed;
+}
+
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h	(revision 8282)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h	(revision 8284)
@@ -1,5 +1,5 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2010 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 This software is provided 'as-is', without any express or implied warranty.
@@ -20,25 +20,46 @@
 #include "LinearMath/btScalar.h"
 #include "btSolverConstraint.h"
-struct  btSolverBody;
-
-
-
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+
+class btSerializer;
 
 enum btTypedConstraintType
 {
-	POINT2POINT_CONSTRAINT_TYPE,
+	POINT2POINT_CONSTRAINT_TYPE=MAX_CONTACT_MANIFOLD_TYPE+1,
 	HINGE_CONSTRAINT_TYPE,
 	CONETWIST_CONSTRAINT_TYPE,
 	D6_CONSTRAINT_TYPE,
-	SLIDER_CONSTRAINT_TYPE
+	SLIDER_CONSTRAINT_TYPE,
+	CONTACT_CONSTRAINT_TYPE
 };
 
+
+enum btConstraintParams
+{
+	BT_CONSTRAINT_ERP=1,
+	BT_CONSTRAINT_STOP_ERP,
+	BT_CONSTRAINT_CFM,
+	BT_CONSTRAINT_STOP_CFM
+};
+
+#if 1
+	#define btAssertConstrParams(_par) btAssert(_par) 
+#else
+	#define btAssertConstrParams(_par)
+#endif
+
+
 ///TypedConstraint is the baseclass for Bullet constraints and vehicles
-class btTypedConstraint
+class btTypedConstraint : public btTypedObject
 {
 	int	m_userConstraintType;
-	int	m_userConstraintId;
-
-	btTypedConstraintType m_constraintType;
+
+	union
+	{
+		int	m_userConstraintId;
+		void* m_userConstraintPtr;
+	};
+
+	bool m_needsFeedback;
 
 	btTypedConstraint&	operator=(btTypedConstraint&	other)
@@ -55,8 +76,11 @@
 	btScalar	m_dbgDrawSize;
 
+	///internal method used by the constraint solver, don't use them directly
+	btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
+	
+	static btRigidBody& getFixedBody();
 
 public:
 
-	btTypedConstraint(btTypedConstraintType type);
 	virtual ~btTypedConstraint() {};
 	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
@@ -94,19 +118,43 @@
 		// the constraint.
 		int *findex;
+		// number of solver iterations
+		int m_numIterations;
+
+		//damping of the velocity
+		btScalar	m_damping;
 	};
 
-
-	virtual void	buildJacobian() = 0;
-
+	///internal method used by the constraint solver, don't use them directly
+	virtual void	buildJacobian() {};
+
+	///internal method used by the constraint solver, don't use them directly
 	virtual	void	setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
 	{
-	}
+        (void)ca;
+        (void)solverBodyA;
+        (void)solverBodyB;
+        (void)timeStep;
+	}
+	
+	///internal method used by the constraint solver, don't use them directly
 	virtual void getInfo1 (btConstraintInfo1* info)=0;
 
+	///internal method used by the constraint solver, don't use them directly
 	virtual void getInfo2 (btConstraintInfo2* info)=0;
 
-	virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep) = 0;
-
-	btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
+	///internal method used by the constraint solver, don't use them directly
+	void	internalSetAppliedImpulse(btScalar appliedImpulse)
+	{
+		m_appliedImpulse = appliedImpulse;
+	}
+	///internal method used by the constraint solver, don't use them directly
+	btScalar	internalGetAppliedImpulse()
+	{
+		return m_appliedImpulse;
+	}
+
+	///internal method used by the constraint solver, don't use them directly
+	virtual	void	solveConstraintObsolete(btRigidBody& /*bodyA*/,btRigidBody& /*bodyB*/,btScalar	/*timeStep*/) {};
+
 	
 	const btRigidBody& getRigidBodyA() const
@@ -148,4 +196,14 @@
 	}
 
+	void	setUserConstraintPtr(void* ptr)
+	{
+		m_userConstraintPtr = ptr;
+	}
+
+	void*	getUserConstraintPtr()
+	{
+		return m_userConstraintPtr;
+	}
+
 	int getUid() const
 	{
@@ -153,6 +211,21 @@
 	} 
 
+	bool	needsFeedback() const
+	{
+		return m_needsFeedback;
+	}
+
+	///enableFeedback will allow to read the applied linear and angular impulse
+	///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
+	void	enableFeedback(bool needsFeedback)
+	{
+		m_needsFeedback = needsFeedback;
+	}
+
+	///getAppliedImpulse is an estimated total applied impulse. 
+	///This feedback could be used to determine breaking constraints or playing sounds.
 	btScalar	getAppliedImpulse() const
 	{
+		btAssert(m_needsFeedback);
 		return m_appliedImpulse;
 	}
@@ -160,5 +233,5 @@
 	btTypedConstraintType getConstraintType () const
 	{
-		return m_constraintType;
+		return btTypedConstraintType(m_objectType);
 	}
 	
@@ -171,6 +244,72 @@
 		return m_dbgDrawSize;
 	}
-	
+
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///If no axis is provided, it uses the default axis for this constraint.
+	virtual	void	setParam(int num, btScalar value, int axis = -1) = 0;
+
+	///return the local value of parameter
+	virtual	btScalar getParam(int num, int axis = -1) const = 0;
+	
+	virtual	int	calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+
 };
 
+// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits 
+// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
+SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
+{
+	if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
+	{
+		return angleInRadians;
+	}
+	else if(angleInRadians < angleLowerLimitInRadians)
+	{
+		btScalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
+		btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
+		return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
+	}
+	else if(angleInRadians > angleUpperLimitInRadians)
+	{
+		btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
+		btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
+		return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
+	}
+	else
+	{
+		return angleInRadians;
+	}
+}
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btTypedConstraintData
+{
+	btRigidBodyData		*m_rbA;
+	btRigidBodyData		*m_rbB;
+	char	*m_name;
+
+	int	m_objectType;
+	int	m_userConstraintType;
+	int	m_userConstraintId;
+	int	m_needsFeedback;
+
+	float	m_appliedImpulse;
+	float	m_dbgDrawSize;
+
+	int	m_disableCollisionsBetweenLinkedBodies;
+	char	m_pad4[4];
+	
+};
+
+SIMD_FORCE_INLINE	int	btTypedConstraint::calculateSerializeBufferSize() const
+{
+	return sizeof(btTypedConstraintData);
+}
+
+
+
+
 #endif //TYPED_CONSTRAINT_H
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp	(revision 8284)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp	(revision 8284)
@@ -0,0 +1,63 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. 
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "btUniversalConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+
+
+
+#define UNIV_EPS btScalar(0.01f)
+
+
+// constructor
+// anchor, axis1 and axis2 are in world coordinate system
+// axis1 must be orthogonal to axis2
+btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
+: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
+ m_anchor(anchor),
+ m_axis1(axis1),
+ m_axis2(axis2)
+{
+	// build frame basis
+	// 6DOF constraint uses Euler angles and to define limits
+	// it is assumed that rotational order is :
+	// Z - first, allowed limits are (-PI,PI);
+	// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number 
+	// used to prevent constraint from instability on poles;
+	// new position of X, allowed limits are (-PI,PI);
+	// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
+	// Build the frame in world coordinate system first
+	btVector3 zAxis = axis1.normalize();
+	btVector3 yAxis = axis2.normalize();
+	btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
+	btTransform frameInW;
+	frameInW.setIdentity();
+	frameInW.getBasis().setValue(	xAxis[0], yAxis[0], zAxis[0],	
+									xAxis[1], yAxis[1], zAxis[1],
+									xAxis[2], yAxis[2], zAxis[2]);
+	frameInW.setOrigin(anchor);
+	// now get constraint frame in local coordinate systems
+	m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
+	m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
+	// sei limits
+	setLinearLowerLimit(btVector3(0., 0., 0.));
+	setLinearUpperLimit(btVector3(0., 0., 0.));
+	setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS));
+	setAngularUpperLimit(btVector3(0.f,  SIMD_HALF_PI - UNIV_EPS,  SIMD_PI - UNIV_EPS));
+}
+
Index: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btUniversalConstraint.h
===================================================================
--- code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btUniversalConstraint.h	(revision 8284)
+++ code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btUniversalConstraint.h	(revision 8284)
@@ -0,0 +1,60 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. 
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef UNIVERSAL_CONSTRAINT_H
+#define UNIVERSAL_CONSTRAINT_H
+
+
+
+#include "LinearMath/btVector3.h"
+#include "btTypedConstraint.h"
+#include "btGeneric6DofConstraint.h"
+
+
+
+/// Constraint similar to ODE Universal Joint
+/// has 2 rotatioonal degrees of freedom, similar to Euler rotations around Z (axis 1)
+/// and Y (axis 2)
+/// Description from ODE manual : 
+/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular. 
+/// In other words, rotation of the two bodies about the direction perpendicular to the two axes will be equal."
+
+class btUniversalConstraint : public btGeneric6DofConstraint
+{
+protected:
+	btVector3	m_anchor;
+	btVector3	m_axis1;
+	btVector3	m_axis2;
+public:
+	// constructor
+	// anchor, axis1 and axis2 are in world coordinate system
+	// axis1 must be orthogonal to axis2
+    btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2);
+	// access
+	const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
+	const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
+	const btVector3& getAxis1() { return m_axis1; }
+	const btVector3& getAxis2() { return m_axis2; }
+	btScalar getAngle1() { return getAngle(2); }
+	btScalar getAngle2() { return getAngle(1); }
+	// limits
+	void setUpperLimit(btScalar ang1max, btScalar ang2max) { setAngularUpperLimit(btVector3(0.f, ang1max, ang2max)); }
+	void setLowerLimit(btScalar ang1min, btScalar ang2min) { setAngularLowerLimit(btVector3(0.f, ang1min, ang2min)); }
+};
+
+
+
+#endif // UNIVERSAL_CONSTRAINT_H
+
