Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp	(revision 2907)
@@ -23,6 +23,14 @@
 #include <new>
 
+//-----------------------------------------------------------------------------
+
+#define CONETWIST_USE_OBSOLETE_SOLVER false
+#define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
+
+//-----------------------------------------------------------------------------
+
 btConeTwistConstraint::btConeTwistConstraint()
-:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE)
+:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
+m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
 {
 }
@@ -32,67 +40,226 @@
 											 const btTransform& rbAFrame,const btTransform& rbBFrame)
 											 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
-											 m_angularOnly(false)
-{
-	m_swingSpan1 = btScalar(1e30);
-	m_swingSpan2 = btScalar(1e30);
-	m_twistSpan  = btScalar(1e30);
-	m_biasFactor = 0.3f;
-	m_relaxationFactor = 1.0f;
-
+											 m_angularOnly(false),
+											 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
+{
+	init();
+}
+
+btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
+											:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
+											 m_angularOnly(false),
+											 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
+{
+	m_rbBFrame = m_rbAFrame;
+	init();	
+}
+
+
+void btConeTwistConstraint::init()
+{
+	m_angularOnly = false;
 	m_solveTwistLimit = false;
 	m_solveSwingLimit = false;
-
-}
-
-btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
-											:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
-											 m_angularOnly(false)
-{
-	m_rbBFrame = m_rbAFrame;
+	m_bMotorEnabled = false;
+	m_maxMotorImpulse = btScalar(-1);
+
+	setLimit(btScalar(1e30), btScalar(1e30), btScalar(1e30));
+	m_damping = btScalar(0.01);
+	m_fixThresh = CONETWIST_DEF_FIX_THRESH;
+}
+
+
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
+	} 
+	else
+	{
+		info->m_numConstraintRows = 3;
+		info->nub = 3;
+		calcAngleInfo2();
+		if(m_solveSwingLimit)
+		{
+			info->m_numConstraintRows++;
+			info->nub--;
+			if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+			{
+				info->m_numConstraintRows++;
+				info->nub--;
+			}
+		}
+		if(m_solveTwistLimit)
+		{
+			info->m_numConstraintRows++;
+			info->nub--;
+		}
+	}
+} // btConeTwistConstraint::getInfo1()
 	
-	m_swingSpan1 = btScalar(1e30);
-	m_swingSpan2 = btScalar(1e30);
-	m_twistSpan  = btScalar(1e30);
-	m_biasFactor = 0.3f;
-	m_relaxationFactor = 1.0f;
-
-	m_solveTwistLimit = false;
-	m_solveSwingLimit = false;
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	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* angular0 = (btVector3*)(info->m_J1angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+		btVector3 a1neg = -a1;
+		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+	btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin();
+	{
+		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+    // set right hand side
+    btScalar k = info->fps * info->erp;
+    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_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
+		info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
+    }
+	int row = 3;
+    int srow = row * info->rowskip;
+	btVector3 ax1;
+	// angular limits
+	if(m_solveSwingLimit)
+	{
+		btScalar *J1 = info->m_J1angularAxis;
+		btScalar *J2 = info->m_J2angularAxis;
+		if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+		{
+			btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
+			btVector3 p = trA.getBasis().getColumn(1);
+			btVector3 q = trA.getBasis().getColumn(2);
+			int srow1 = srow + info->rowskip;
+			J1[srow+0] = p[0];
+			J1[srow+1] = p[1];
+			J1[srow+2] = p[2];
+			J1[srow1+0] = q[0];
+			J1[srow1+1] = q[1];
+			J1[srow1+2] = q[2];
+			J2[srow+0] = -p[0];
+			J2[srow+1] = -p[1];
+			J2[srow+2] = -p[2];
+			J2[srow1+0] = -q[0];
+			J2[srow1+1] = -q[1];
+			J2[srow1+2] = -q[2];
+			btScalar fact = info->fps * m_relaxationFactor;
+			info->m_constraintError[srow] =   fact * m_swingAxis.dot(p);
+			info->m_constraintError[srow1] =  fact * m_swingAxis.dot(q);
+			info->m_lowerLimit[srow] = -SIMD_INFINITY;
+			info->m_upperLimit[srow] = SIMD_INFINITY;
+			info->m_lowerLimit[srow1] = -SIMD_INFINITY;
+			info->m_upperLimit[srow1] = SIMD_INFINITY;
+			srow = srow1 + info->rowskip;
+		}
+		else
+		{
+			ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
+			J1[srow+0] = ax1[0];
+			J1[srow+1] = ax1[1];
+			J1[srow+2] = ax1[2];
+			J2[srow+0] = -ax1[0];
+			J2[srow+1] = -ax1[1];
+			J2[srow+2] = -ax1[2];
+			btScalar k = info->fps * m_biasFactor;
+
+			info->m_constraintError[srow] = k * m_swingCorrection;
+			info->cfm[srow] = 0.0f;
+			// m_swingCorrection is always positive or 0
+			info->m_lowerLimit[srow] = 0;
+			info->m_upperLimit[srow] = SIMD_INFINITY;
+			srow += info->rowskip;
+		}
+	}
+	if(m_solveTwistLimit)
+	{
+		ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
+		btScalar *J1 = info->m_J1angularAxis;
+		btScalar *J2 = info->m_J2angularAxis;
+		J1[srow+0] = ax1[0];
+		J1[srow+1] = ax1[1];
+		J1[srow+2] = ax1[2];
+		J2[srow+0] = -ax1[0];
+		J2[srow+1] = -ax1[1];
+		J2[srow+2] = -ax1[2];
+		btScalar k = info->fps * m_biasFactor;
+		info->m_constraintError[srow] = k * m_twistCorrection;
+		info->cfm[srow] = 0.0f;
+		if(m_twistSpan > 0.0f)
+		{
+
+			if(m_twistCorrection > 0.0f)
+			{
+				info->m_lowerLimit[srow] = 0;
+				info->m_upperLimit[srow] = SIMD_INFINITY;
+			} 
+			else
+			{
+				info->m_lowerLimit[srow] = -SIMD_INFINITY;
+				info->m_upperLimit[srow] = 0;
+			} 
+		}
+		else
+		{
+			info->m_lowerLimit[srow] = -SIMD_INFINITY;
+			info->m_upperLimit[srow] = SIMD_INFINITY;
+		}
+		srow += info->rowskip;
+	}
+}
 	
-}			
+//-----------------------------------------------------------------------------
 
 void	btConeTwistConstraint::buildJacobian()
 {
-	m_appliedImpulse = btScalar(0.);
-
-	//set bias, sign, clear accumulator
-	m_swingCorrection = btScalar(0.);
-	m_twistLimitSign = btScalar(0.);
-	m_solveTwistLimit = false;
-	m_solveSwingLimit = false;
-	m_accTwistLimitImpulse = btScalar(0.);
-	m_accSwingLimitImpulse = btScalar(0.);
-
-	if (!m_angularOnly)
-	{
-		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
-		btVector3 relPos = pivotBInW - pivotAInW;
-
-		btVector3 normal[3];
-		if (relPos.length2() > SIMD_EPSILON)
-		{
-			normal[0] = relPos.normalized();
-		}
-		else
-		{
-			normal[0].setValue(btScalar(1.0),0,0);
-		}
-
-		btPlaneSpace1(normal[0], normal[1], normal[2]);
-
-		for (int i=0;i<3;i++)
-		{
-			new (&m_jac[i]) btJacobianEntry(
+	if (m_useSolveConstraintObsolete)
+	{
+		m_appliedImpulse = btScalar(0.);
+		m_accTwistLimitImpulse = btScalar(0.);
+		m_accSwingLimitImpulse = btScalar(0.);
+
+		if (!m_angularOnly)
+		{
+			btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+			btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+			btVector3 relPos = pivotBInW - pivotAInW;
+
+			btVector3 normal[3];
+			if (relPos.length2() > SIMD_EPSILON)
+			{
+				normal[0] = relPos.normalized();
+			}
+			else
+			{
+				normal[0].setValue(btScalar(1.0),0,0);
+			}
+
+			btPlaneSpace1(normal[0], normal[1], normal[2]);
+
+			for (int i=0;i<3;i++)
+			{
+				new (&m_jac[i]) btJacobianEntry(
 				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
 				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -104,6 +271,240 @@
 				m_rbB.getInvInertiaDiagLocal(),
 				m_rbB.getInvMass());
-		}
-	}
+			}
+		}
+
+		calcAngleInfo2();
+	}
+}
+
+//-----------------------------------------------------------------------------
+
+void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
+{
+	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;
+			bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+			btVector3 vel2;
+			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 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);
+		
+			}
+		}
+
+		// apply motor
+		if (m_bMotorEnabled)
+		{
+			// compute current and predicted transforms
+			btTransform trACur = m_rbA.getCenterOfMassTransform();
+			btTransform trBCur = m_rbB.getCenterOfMassTransform();
+			btVector3 omegaA; bodyA.getAngularVelocity(omegaA);
+			btVector3 omegaB; bodyB.getAngularVelocity(omegaB);
+			btTransform trAPred; trAPred.setIdentity(); 
+			btVector3 zerovec(0,0,0);
+			btTransformUtil::integrateTransform(
+				trACur, zerovec, omegaA, timeStep, trAPred);
+			btTransform trBPred; trBPred.setIdentity(); 
+			btTransformUtil::integrateTransform(
+				trBCur, zerovec, omegaB, timeStep, trBPred);
+
+			// compute desired transforms in world
+			btTransform trPose(m_qTarget);
+			btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
+			btTransform trADes = trBPred * trABDes;
+			btTransform trBDes = trAPred * trABDes.inverse();
+
+			// compute desired omegas in world
+			btVector3 omegaADes, omegaBDes;
+			
+			btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
+			btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
+
+			// compute delta omegas
+			btVector3 dOmegaA = omegaADes - omegaA;
+			btVector3 dOmegaB = omegaBDes - omegaB;
+
+			// compute weighted avg axis of dOmega (weighting based on inertias)
+			btVector3 axisA, axisB;
+			btScalar kAxisAInv = 0, kAxisBInv = 0;
+
+			if (dOmegaA.length2() > SIMD_EPSILON)
+			{
+				axisA = dOmegaA.normalized();
+				kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
+			}
+
+			if (dOmegaB.length2() > SIMD_EPSILON)
+			{
+				axisB = dOmegaB.normalized();
+				kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
+			}
+
+			btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
+
+			static bool bDoTorque = true;
+			if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
+			{
+				avgAxis.normalize();
+				kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
+				kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
+				btScalar kInvCombined = kAxisAInv + kAxisBInv;
+
+				btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
+									(kInvCombined * kInvCombined);
+
+				if (m_maxMotorImpulse >= 0)
+				{
+					btScalar fMaxImpulse = m_maxMotorImpulse;
+					if (m_bNormalizedMotorStrength)
+						fMaxImpulse = fMaxImpulse/kAxisAInv;
+
+					btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
+					btScalar  newUnclampedMag = newUnclampedAccImpulse.length();
+					if (newUnclampedMag > fMaxImpulse)
+					{
+						newUnclampedAccImpulse.normalize();
+						newUnclampedAccImpulse *= fMaxImpulse;
+						impulse = newUnclampedAccImpulse - m_accMotorImpulse;
+					}
+					m_accMotorImpulse += impulse;
+				}
+
+				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);
+
+			}
+		}
+		else // no motor: do a little damping
+		{
+			const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
+			const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
+			btVector3 relVel = angVelB - angVelA;
+			if (relVel.length2() > SIMD_EPSILON)
+			{
+				btVector3 relVelAxis = relVel.normalized();
+				btScalar m_kDamping =  btScalar(1.) /
+					(getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
+					 getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
+				btVector3 impulse = m_damping * m_kDamping * relVel;
+
+				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);
+			}
+		}
+
+		// joint limits
+		{
+			///solve angular part
+			btVector3 angVelA;
+			bodyA.getAngularVelocity(angVelA);
+			btVector3 angVelB;
+			bodyB.getAngularVelocity(angVelB);
+
+			// solve swing limit
+			if (m_solveSwingLimit)
+			{
+				btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
+				btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
+				if (relSwingVel > 0)
+					amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
+				btScalar impulseMag = amplitude * m_kSwing;
+
+				// Clamp the accumulated impulse
+				btScalar temp = m_accSwingLimitImpulse;
+				m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
+				impulseMag = m_accSwingLimitImpulse - temp;
+
+				btVector3 impulse = m_swingAxis * impulseMag;
+
+				// don't let cone response affect twist
+				// (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
+				{
+					btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
+					btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
+					impulse = impulseNoTwistCouple;
+				}
+
+				impulseMag = impulse.length();
+				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);
+			}
+
+
+			// solve twist limit
+			if (m_solveTwistLimit)
+			{
+				btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
+				btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis );
+				if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
+					amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
+				btScalar impulseMag = amplitude * m_kTwist;
+
+				// Clamp the accumulated impulse
+				btScalar temp = m_accTwistLimitImpulse;
+				m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
+				impulseMag = m_accTwistLimitImpulse - temp;
+
+				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);
+			}		
+		}
+	}
+
+}
+
+//-----------------------------------------------------------------------------
+
+void	btConeTwistConstraint::updateRHS(btScalar	timeStep)
+{
+	(void)timeStep;
+
+}
+
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::calcAngleInfo()
+{
+	m_swingCorrection = btScalar(0.);
+	m_twistLimitSign = btScalar(0.);
+	m_solveTwistLimit = false;
+	m_solveSwingLimit = false;
 
 	btVector3 b1Axis1,b1Axis2,b1Axis3;
@@ -123,5 +524,4 @@
 	{
 		b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
-//		swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
 		swx = b2Axis1.dot(b1Axis1);
 		swy = b2Axis1.dot(b1Axis2);
@@ -130,5 +530,4 @@
 		fact = fact / (fact + btScalar(1.0));
 		swing1 *= fact; 
-
 	}
 
@@ -136,5 +535,4 @@
 	{
 		b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);			
-//		swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
 		swx = b2Axis1.dot(b1Axis1);
 		swy = b2Axis1.dot(b1Axis3);
@@ -153,15 +551,9 @@
 		m_swingCorrection = EllipseAngle-1.0f;
 		m_solveSwingLimit = true;
-		
 		// Calculate necessary axis & factors
 		m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
 		m_swingAxis.normalize();
-
 		btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
 		m_swingAxis *= swingAxisSign;
-
-		m_kSwing =  btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
-			getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
-
 	}
 
@@ -173,117 +565,425 @@
 		btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 
 		btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
-
-		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
+		m_twistAngle = twist;
+
+//		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
+		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
 		if (twist <= -m_twistSpan*lockedFreeFactor)
 		{
 			m_twistCorrection = -(twist + m_twistSpan);
 			m_solveTwistLimit = true;
-
 			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
 			m_twistAxis.normalize();
 			m_twistAxis *= -1.0f;
-
-			m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
-				getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
-
-		}	else
-			if (twist >  m_twistSpan*lockedFreeFactor)
-			{
-				m_twistCorrection = (twist - m_twistSpan);
+		}
+		else if (twist >  m_twistSpan*lockedFreeFactor)
+		{
+			m_twistCorrection = (twist - m_twistSpan);
+			m_solveTwistLimit = true;
+			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
+			m_twistAxis.normalize();
+		}
+	}
+} // btConeTwistConstraint::calcAngleInfo()
+
+
+static btVector3 vTwist(1,0,0); // twist axis in constraint's space
+
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::calcAngleInfo2()
+{
+	m_swingCorrection = btScalar(0.);
+	m_twistLimitSign = btScalar(0.);
+	m_solveTwistLimit = false;
+	m_solveSwingLimit = false;
+
+	{
+		// 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 qAB = qB.inverse() * qA;
+
+		// split rotation into cone and twist
+		// (all this is done from B's perspective. Maybe I should be averaging axes...)
+		btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
+		btQuaternion qABCone  = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
+		btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
+
+		if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
+		{
+			btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
+			computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
+
+			if (swingAngle > swingLimit * m_limitSoftness)
+			{
+				m_solveSwingLimit = true;
+
+				// compute limit ratio: 0->1, where
+				// 0 == beginning of soft limit
+				// 1 == hard/real limit
+				m_swingLimitRatio = 1.f;
+				if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
+				{
+					m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
+										(swingLimit - swingLimit * m_limitSoftness);
+				}				
+
+				// swing correction tries to get back to soft limit
+				m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
+
+				// adjustment of swing axis (based on ellipse normal)
+				adjustSwingAxisToUseEllipseNormal(swingAxis);
+
+				// Calculate necessary axis & factors		
+				m_swingAxis = quatRotate(qB, -swingAxis);
+
+				m_twistAxisA.setValue(0,0,0);
+
+				m_kSwing =  btScalar(1.) /
+					(getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
+					 getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
+			}
+		}
+		else
+		{
+			// you haven't set any limits;
+			// 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 target;
+			btScalar x = ivB.dot(ivA);
+			btScalar y = ivB.dot(jvA);
+			btScalar z = ivB.dot(kvA);
+			if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+			{ // fixed. We'll need to add one more row to constraint
+				if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
+				{
+					m_solveSwingLimit = true;
+					m_swingAxis = -ivB.cross(ivA);
+				}
+			}
+			else
+			{
+				if(m_swingSpan1 < m_fixThresh)
+				{ // hinge around Y axis
+					if(!(btFuzzyZero(y)))
+					{
+						m_solveSwingLimit = true;
+						if(m_swingSpan2 >= m_fixThresh)
+						{
+							y = btScalar(0.f);
+							btScalar span2 = btAtan2(z, x);
+							if(span2 > m_swingSpan2)
+							{
+								x = btCos(m_swingSpan2);
+								z = btSin(m_swingSpan2);
+							}
+							else if(span2 < -m_swingSpan2)
+							{
+								x =  btCos(m_swingSpan2);
+								z = -btSin(m_swingSpan2);
+							}
+						}
+					}
+				}
+				else
+				{ // hinge around Z axis
+					if(!btFuzzyZero(z))
+					{
+						m_solveSwingLimit = true;
+						if(m_swingSpan1 >= m_fixThresh)
+						{
+							z = btScalar(0.f);
+							btScalar span1 = btAtan2(y, x);
+							if(span1 > m_swingSpan1)
+							{
+								x = btCos(m_swingSpan1);
+								y = btSin(m_swingSpan1);
+							}
+							else if(span1 < -m_swingSpan1)
+							{
+								x =  btCos(m_swingSpan1);
+								y = -btSin(m_swingSpan1);
+							}
+						}
+					}
+				}
+				target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
+				target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
+				target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
+				target.normalize();
+				m_swingAxis = -ivB.cross(target);
+				m_swingCorrection = m_swingAxis.length();
+				m_swingAxis.normalize();
+			}
+		}
+
+		if (m_twistSpan >= btScalar(0.f))
+		{
+			btVector3 twistAxis;
+			computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
+
+			if (m_twistAngle > m_twistSpan*m_limitSoftness)
+			{
 				m_solveTwistLimit = true;
 
-				m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
-				m_twistAxis.normalize();
-
-				m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
-					getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
-
-			}
-	}
-}
-
-void	btConeTwistConstraint::solveConstraint(btScalar	timeStep)
-{
-
-	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 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-		btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-		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;
-			m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
-			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
-		}
-	}
-	
-	{
-		///solve angular part
-		const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
-		const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
-
-		// solve swing limit
-		if (m_solveSwingLimit)
-		{
-			btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
-			btScalar impulseMag = amplitude * m_kSwing;
-
-			// Clamp the accumulated impulse
-			btScalar temp = m_accSwingLimitImpulse;
-			m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
-			impulseMag = m_accSwingLimitImpulse - temp;
-
-			btVector3 impulse = m_swingAxis * impulseMag;
-
-			m_rbA.applyTorqueImpulse(impulse);
-			m_rbB.applyTorqueImpulse(-impulse);
-
-		}
-
-		// solve twist limit
-		if (m_solveTwistLimit)
-		{
-			btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor );
-			btScalar impulseMag = amplitude * m_kTwist;
-
-			// Clamp the accumulated impulse
-			btScalar temp = m_accTwistLimitImpulse;
-			m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
-			impulseMag = m_accTwistLimitImpulse - temp;
-
-			btVector3 impulse = m_twistAxis * impulseMag;
-
-			m_rbA.applyTorqueImpulse(impulse);
-			m_rbB.applyTorqueImpulse(-impulse);
-
-		}
-	
-	}
-
-}
-
-void	btConeTwistConstraint::updateRHS(btScalar	timeStep)
-{
-	(void)timeStep;
-
-}
+				m_twistLimitRatio = 1.f;
+				if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
+				{
+					m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
+										(m_twistSpan  - m_twistSpan * m_limitSoftness);
+				}
+
+				// twist correction tries to get back to soft limit
+				m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
+
+				m_twistAxis = quatRotate(qB, -twistAxis);
+
+				m_kTwist = btScalar(1.) /
+					(getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
+					 getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
+			}
+
+			if (m_solveSwingLimit)
+				m_twistAxisA = quatRotate(qA, -twistAxis);
+		}
+		else
+		{
+			m_twistAngle = btScalar(0.f);
+		}
+	}
+} // btConeTwistConstraint::calcAngleInfo2()
+
+
+
+// given a cone rotation in constraint space, (pre: twist must already be removed)
+// this method computes its corresponding swing angle and axis.
+// more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
+void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
+												 btScalar& swingAngle, // out
+												 btVector3& vSwingAxis, // out
+												 btScalar& swingLimit) // out
+{
+	swingAngle = qCone.getAngle();
+	if (swingAngle > SIMD_EPSILON)
+	{
+		vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
+		vSwingAxis.normalize();
+		if (fabs(vSwingAxis.x()) > SIMD_EPSILON)
+		{
+			// non-zero twist?! this should never happen.
+			int wtf = 0; wtf = wtf;
+		}
+
+		// Compute limit for given swing. tricky:
+		// Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
+		// (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
+
+		// For starters, compute the direction from center to surface of ellipse.
+		// This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
+		// (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
+		btScalar xEllipse =  vSwingAxis.y();
+		btScalar yEllipse = -vSwingAxis.z();
+
+		// Now, we use the slope of the vector (using x/yEllipse) and find the length
+		// of the line that intersects the ellipse:
+		//  x^2   y^2
+		//  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
+		//  a^2   b^2
+		// Do the math and it should be clear.
+
+		swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
+		if (fabs(xEllipse) > SIMD_EPSILON)
+		{
+			btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
+			btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
+			norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
+			btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
+			swingLimit = sqrt(swingLimit2);
+		}
+
+		// test!
+		/*swingLimit = m_swingSpan2;
+		if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
+		{
+		btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
+		btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
+		btScalar phi = asin(sinphi);
+		btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
+		btScalar alpha = 3.14159f - theta - phi;
+		btScalar sinalpha = sin(alpha);
+		swingLimit = m_swingSpan1 * sinphi/sinalpha;
+		}*/
+	}
+	else if (swingAngle < 0)
+	{
+		// this should never happen!
+		int wtf = 0; wtf = wtf;
+	}
+}
+
+btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
+{
+	// compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
+	btScalar xEllipse = btCos(fAngleInRadians);
+	btScalar yEllipse = btSin(fAngleInRadians);
+
+	// Use the slope of the vector (using x/yEllipse) and find the length
+	// of the line that intersects the ellipse:
+	//  x^2   y^2
+	//  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
+	//  a^2   b^2
+	// Do the math and it should be clear.
+
+	float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
+	if (fabs(xEllipse) > SIMD_EPSILON)
+	{
+		btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
+		btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
+		norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
+		btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
+		swingLimit = sqrt(swingLimit2);
+	}
+
+	// convert into point in constraint space:
+	// note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
+	btVector3 vSwingAxis(0, xEllipse, -yEllipse);
+	btQuaternion qSwing(vSwingAxis, swingLimit);
+	btVector3 vPointInConstraintSpace(fLength,0,0);
+	return quatRotate(qSwing, vPointInConstraintSpace);
+}
+
+// given a twist rotation in constraint space, (pre: cone must already be removed)
+// this method computes its corresponding angle and axis.
+void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
+												  btScalar& twistAngle, // out
+												  btVector3& vTwistAxis) // out
+{
+	btQuaternion qMinTwist = qTwist;
+	twistAngle = qTwist.getAngle();
+
+	if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
+	{
+		qMinTwist = operator-(qTwist);
+		twistAngle = qMinTwist.getAngle();
+	}
+	if (twistAngle < 0)
+	{
+		// this should never happen
+		int wtf = 0; wtf = wtf;			
+	}
+
+	vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
+	if (twistAngle > SIMD_EPSILON)
+		vTwistAxis.normalize();
+}
+
+
+void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
+{
+	// the swing axis is computed as the "twist-free" cone rotation,
+	// but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
+	// so, if we're outside the limits, the closest way back inside the cone isn't 
+	// along the vector back to the center. better (and more stable) to use the ellipse normal.
+
+	// convert swing axis to direction from center to surface of ellipse
+	// (ie. rotate 2D vector by PI/2)
+	btScalar y = -vSwingAxis.z();
+	btScalar z =  vSwingAxis.y();
+
+	// do the math...
+	if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
+	{
+		// compute gradient/normal of ellipse surface at current "point"
+		btScalar grad = y/z;
+		grad *= m_swingSpan2 / m_swingSpan1;
+
+		// adjust y/z to represent normal at point (instead of vector to point)
+		if (y > 0)
+			y =  fabs(grad * z);
+		else
+			y = -fabs(grad * z);
+
+		// convert ellipse direction back to swing axis
+		vSwingAxis.setZ(-y);
+		vSwingAxis.setY( z);
+		vSwingAxis.normalize();
+	}
+}
+
+
+
+void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
+{
+	btTransform trACur = m_rbA.getCenterOfMassTransform();
+	btTransform trBCur = m_rbB.getCenterOfMassTransform();
+	btTransform trABCur = trBCur.inverse() * trACur;
+	btQuaternion qABCur = trABCur.getRotation();
+	btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
+	btQuaternion qConstraintCur = trConstraintCur.getRotation();
+
+	btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
+	setMotorTargetInConstraintSpace(qConstraint);
+}
+
+
+void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
+{
+	m_qTarget = q;
+
+	// clamp motor target to within limits
+	{
+		btScalar softness = 1.f;//m_limitSoftness;
+
+		// split into twist and cone
+		btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
+		btQuaternion qTargetCone  = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
+		btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();
+
+		// clamp cone
+		if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
+		{
+			btScalar swingAngle, swingLimit; btVector3 swingAxis;
+			computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
+
+			if (fabs(swingAngle) > SIMD_EPSILON)
+			{
+				if (swingAngle > swingLimit*softness)
+					swingAngle = swingLimit*softness;
+				else if (swingAngle < -swingLimit*softness)
+					swingAngle = -swingLimit*softness;
+				qTargetCone = btQuaternion(swingAxis, swingAngle);
+			}
+		}
+
+		// clamp twist
+		if (m_twistSpan >= btScalar(0.05f))
+		{
+			btScalar twistAngle; btVector3 twistAxis;
+			computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
+
+			if (fabs(twistAngle) > SIMD_EPSILON)
+			{
+				// eddy todo: limitSoftness used here???
+				if (twistAngle > m_twistSpan*softness)
+					twistAngle = m_twistSpan*softness;
+				else if (twistAngle < -m_twistSpan*softness)
+					twistAngle = -m_twistSpan*softness;
+				qTargetTwist = btQuaternion(twistAxis, twistAngle);
+			}
+		}
+
+		m_qTarget = qTargetCone * qTargetTwist;
+	}
+}
+
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+
+
Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h	(revision 2907)
@@ -43,8 +43,12 @@
 	btScalar	m_relaxationFactor;
 
+	btScalar	m_damping;
+
 	btScalar	m_swingSpan1;
 	btScalar	m_swingSpan2;
 	btScalar	m_twistSpan;
 
+	btScalar	m_fixThresh;
+
 	btVector3   m_swingAxis;
 	btVector3	m_twistAxis;
@@ -57,4 +61,6 @@
 	btScalar	m_twistCorrection;
 
+	btScalar	m_twistAngle;
+
 	btScalar	m_accSwingLimitImpulse;
 	btScalar	m_accTwistLimitImpulse;
@@ -64,4 +70,17 @@
 	bool		m_solveSwingLimit;
 
+	bool	m_useSolveConstraintObsolete;
+
+	// not yet used...
+	btScalar	m_swingLimitRatio;
+	btScalar	m_twistLimitRatio;
+	btVector3   m_twistAxisA;
+
+	// motor
+	bool		 m_bMotorEnabled;
+	bool		 m_bNormalizedMotorStrength;
+	btQuaternion m_qTarget;
+	btScalar	 m_maxMotorImpulse;
+	btVector3	 m_accMotorImpulse;
 	
 public:
@@ -75,5 +94,10 @@
 	virtual void	buildJacobian();
 
-	virtual	void	solveConstraint(btScalar	timeStep);
+	virtual void getInfo1 (btConstraintInfo1* info);
+	
+	virtual void getInfo2 (btConstraintInfo2* info);
+	
+
+	virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
 
 	void	updateRHS(btScalar	timeStep);
@@ -93,5 +117,30 @@
 	}
 
-	void	setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 0.8f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+	void	setLimit(int limitIndex,btScalar limitValue)
+	{
+		switch (limitIndex)
+		{
+		case 3:
+			{
+				m_twistSpan = limitValue;
+				break;
+			}
+		case 4:
+			{
+				m_swingSpan2 = limitValue;
+				break;
+			}
+		case 5:
+			{
+				m_swingSpan1 = limitValue;
+				break;
+			}
+		default:
+			{
+			}
+		};
+	}
+
+	void	setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
 	{
 		m_swingSpan1 = _swingSpan1;
@@ -122,4 +171,58 @@
 	}
 
+	void calcAngleInfo();
+	void calcAngleInfo2();
+
+	inline btScalar getSwingSpan1()
+	{
+		return m_swingSpan1;
+	}
+	inline btScalar getSwingSpan2()
+	{
+		return m_swingSpan2;
+	}
+	inline btScalar getTwistSpan()
+	{
+		return m_twistSpan;
+	}
+	inline btScalar getTwistAngle()
+	{
+		return m_twistAngle;
+	}
+	bool isPastSwingLimit() { return m_solveSwingLimit; }
+
+
+	void setDamping(btScalar damping) { m_damping = damping; }
+
+	void enableMotor(bool b) { m_bMotorEnabled = b; }
+	void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
+	void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
+
+	btScalar getFixThresh() { return m_fixThresh; }
+	void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
+
+	// setMotorTarget:
+	// q: the desired rotation of bodyA wrt bodyB.
+	// note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
+	// note: don't forget to enableMotor()
+	void setMotorTarget(const btQuaternion &q);
+
+	// same as above, but q is the desired rotation of frameA wrt frameB in constraint space
+	void setMotorTargetInConstraintSpace(const btQuaternion &q);
+
+	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;
 };
 
Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp	(revision 2907)
@@ -23,5 +23,5 @@
 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
 
-#define ASSERT2 assert
+#define ASSERT2 btAssert
 
 #define USE_INTERNAL_APPLY_IMPULSE 1
@@ -53,5 +53,5 @@
 	
 
-	  btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
+	   btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
 		body2.getCenterOfMassTransform().getBasis().transpose(),
 		rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
@@ -115,5 +115,5 @@
 
 	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-	assert(cpd);
+	btAssert(cpd);
 	btScalar distance = cpd->m_penetration;
 	btScalar positionalError = Kcor *-distance;
@@ -167,5 +167,5 @@
 	
 	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-	assert(cpd);
+	btAssert(cpd);
 
 	btScalar combinedFriction = cpd->m_friction;
@@ -256,5 +256,5 @@
 	
 	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-	assert(cpd);
+	btAssert(cpd);
 
 	btScalar combinedFriction = cpd->m_friction;
@@ -338,5 +338,5 @@
 
 	btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
-	assert(cpd);
+	btAssert(cpd);
 	btScalar distance = cpd->m_penetration;
 	btScalar positionalError = Kcor *-distance;
Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h	(revision 2907)
@@ -23,5 +23,10 @@
 	SOLVER_USE_WARMSTARTING = 4,
 	SOLVER_USE_FRICTION_WARMSTARTING = 8,
-	SOLVER_CACHE_FRIENDLY = 16
+	SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
+	SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
+	SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
+	SOLVER_CACHE_FRIENDLY = 128,
+	SOLVER_SIMD = 256,	//enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version
+	SOLVER_CUDA = 512	//will be open sourced during Game Developers Conference 2009. Much faster.
 };
 
@@ -40,4 +45,5 @@
 	btScalar	m_erp;//used as Baumgarte factor
 	btScalar	m_erp2;//used in Split Impulse
+	btScalar	m_globalCfm;//constraint force mixing
 	int			m_splitImpulse;
 	btScalar	m_splitImpulsePenetrationThreshold;
@@ -66,10 +72,11 @@
 		m_erp = btScalar(0.2);
 		m_erp2 = btScalar(0.1);
-		m_sor = btScalar(1.3);
+		m_globalCfm = btScalar(0.);
+		m_sor = btScalar(1.);
 		m_splitImpulse = false;
 		m_splitImpulsePenetrationThreshold = -0.02f;
 		m_linearSlop = btScalar(0.0);
 		m_warmstartingFactor=btScalar(0.85);
-		m_solverMode = SOLVER_CACHE_FRIENDLY |  SOLVER_RANDMIZE_ORDER |  SOLVER_USE_WARMSTARTING;
+		m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD ;//SOLVER_RANDMIZE_ORDER
 		m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
 	}
Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp	(revision 2907)
@@ -20,5 +20,4 @@
 */
 
-
 #include "btGeneric6DofConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
@@ -27,5 +26,31 @@
 
 
+#define D6_USE_OBSOLETE_METHOD false
+//-----------------------------------------------------------------------------
+
+btGeneric6DofConstraint::btGeneric6DofConstraint()
+:btTypedConstraint(D6_CONSTRAINT_TYPE),
+m_useLinearReferenceFrameA(true),
+m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
+{
+}
+
+//-----------------------------------------------------------------------------
+
+btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
+: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
+, m_frameInA(frameInA)
+, m_frameInB(frameInB),
+m_useLinearReferenceFrameA(useLinearReferenceFrameA),
+m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
+{
+
+}
+//-----------------------------------------------------------------------------
+
+
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
+
+//-----------------------------------------------------------------------------
 
 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
@@ -37,48 +62,45 @@
 }
 
+//-----------------------------------------------------------------------------
+
 ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
 bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
 bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
 {
-//	// rot =  cy*cz          -cy*sz           sy
-//	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
-//	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
-//
-
-		if (btGetMatrixElem(mat,2) < btScalar(1.0))
-		{
-			if (btGetMatrixElem(mat,2) > btScalar(-1.0))
-			{
-				xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
-				xyz[1] = btAsin(btGetMatrixElem(mat,2));
-				xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
-				return true;
-			}
-			else
-			{
-				// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
-				xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
-				xyz[1] = -SIMD_HALF_PI;
-				xyz[2] = btScalar(0.0);
-				return false;
-			}
+	//	// rot =  cy*cz          -cy*sz           sy
+	//	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
+	//	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
+	//
+
+	btScalar fi = btGetMatrixElem(mat,2);
+	if (fi < btScalar(1.0f))
+	{
+		if (fi > btScalar(-1.0f))
+		{
+			xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
+			xyz[1] = btAsin(btGetMatrixElem(mat,2));
+			xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+			return true;
 		}
 		else
 		{
-			// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
-			xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
-			xyz[1] = SIMD_HALF_PI;
-			xyz[2] = 0.0;
-
-		}
-
-
+			// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
+			xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+			xyz[1] = -SIMD_HALF_PI;
+			xyz[2] = btScalar(0.0);
+			return false;
+		}
+	}
+	else
+	{
+		// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
+		xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+		xyz[1] = SIMD_HALF_PI;
+		xyz[2] = 0.0;
+	}
 	return false;
 }
 
-
-
 //////////////////////////// btRotationalLimitMotor ////////////////////////////////////
-
 
 int btRotationalLimitMotor::testLimitValue(btScalar test_value)
@@ -105,83 +127,88 @@
 	m_currentLimit = 0;//Free from violation
 	return 0;
-	
-}
-
+
+}
+
+//-----------------------------------------------------------------------------
 
 btScalar btRotationalLimitMotor::solveAngularLimits(
-		btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
-	 	btRigidBody * body0, btRigidBody * body1)
-{
-    if (needApplyTorques()==false) return 0.0f;
-
-    btScalar target_velocity = m_targetVelocity;
-    btScalar maxMotorForce = m_maxMotorForce;
+	btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
+	btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
+{
+	if (needApplyTorques()==false) return 0.0f;
+
+	btScalar target_velocity = m_targetVelocity;
+	btScalar maxMotorForce = m_maxMotorForce;
 
 	//current error correction
-    if (m_currentLimit!=0)
-    {
-        target_velocity = -m_ERP*m_currentLimitError/(timeStep);
-        maxMotorForce = m_maxLimitForce;
-    }
-
-    maxMotorForce *= timeStep;
-
-    // current velocity difference
-    btVector3 vel_diff = body0->getAngularVelocity();
-    if (body1)
-    {
-        vel_diff -= body1->getAngularVelocity();
-    }
-
-
-
-    btScalar rel_vel = axis.dot(vel_diff);
+	if (m_currentLimit!=0)
+	{
+		target_velocity = -m_ERP*m_currentLimitError/(timeStep);
+		maxMotorForce = m_maxLimitForce;
+	}
+
+	maxMotorForce *= timeStep;
+
+	// current velocity difference
+
+	btVector3 angVelA;
+	bodyA.getAngularVelocity(angVelA);
+	btVector3 angVelB;
+	bodyB.getAngularVelocity(angVelB);
+
+	btVector3 vel_diff;
+	vel_diff = angVelA-angVelB;
+
+
+
+	btScalar rel_vel = axis.dot(vel_diff);
 
 	// correction velocity
-    btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
-
-
-    if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
-    {
-        return 0.0f;//no need for applying force
-    }
+	btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
+
+
+	if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
+	{
+		return 0.0f;//no need for applying force
+	}
 
 
 	// correction impulse
-    btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
+	btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
 
 	// clip correction impulse
-    btScalar clippedMotorImpulse;
-
-    ///@todo: should clip against accumulated impulse
-    if (unclippedMotorImpulse>0.0f)
-    {
-        clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
-    }
-    else
-    {
-        clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
-    }
+	btScalar clippedMotorImpulse;
+
+	///@todo: should clip against accumulated impulse
+	if (unclippedMotorImpulse>0.0f)
+	{
+		clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
+	}
+	else
+	{
+		clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
+	}
 
 
 	// sort with accumulated impulses
-    btScalar	lo = btScalar(-1e30);
-    btScalar	hi = btScalar(1e30);
-
-    btScalar oldaccumImpulse = m_accumulatedImpulse;
-    btScalar sum = oldaccumImpulse + clippedMotorImpulse;
-    m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
-
-    clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
-
-
-
-    btVector3 motorImp = clippedMotorImpulse * axis;
-
-
-    body0->applyTorqueImpulse(motorImp);
-    if (body1) body1->applyTorqueImpulse(-motorImp);
-
-    return clippedMotorImpulse;
+	btScalar	lo = btScalar(-1e30);
+	btScalar	hi = btScalar(1e30);
+
+	btScalar oldaccumImpulse = m_accumulatedImpulse;
+	btScalar sum = oldaccumImpulse + clippedMotorImpulse;
+	m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
+
+	clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
+
+	btVector3 motorImp = clippedMotorImpulse * axis;
+
+	//body0->applyTorqueImpulse(motorImp);
+	//body1->applyTorqueImpulse(-motorImp);
+
+	bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
+	bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
+
+
+	return clippedMotorImpulse;
 
 
@@ -190,125 +217,147 @@
 //////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
 
+
+
+
 //////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
+
+
+int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
+{
+	btScalar loLimit = m_lowerLimit[limitIndex];
+	btScalar hiLimit = m_upperLimit[limitIndex];
+	if(loLimit > hiLimit)
+	{
+		m_currentLimit[limitIndex] = 0;//Free from violation
+		m_currentLimitError[limitIndex] = btScalar(0.f);
+		return 0;
+	}
+
+	if (test_value < loLimit)
+	{
+		m_currentLimit[limitIndex] = 2;//low limit violation
+		m_currentLimitError[limitIndex] =  test_value - loLimit;
+		return 2;
+	}
+	else if (test_value> hiLimit)
+	{
+		m_currentLimit[limitIndex] = 1;//High limit violation
+		m_currentLimitError[limitIndex] = test_value - hiLimit;
+		return 1;
+	};
+
+	m_currentLimit[limitIndex] = 0;//Free from violation
+	m_currentLimitError[limitIndex] = btScalar(0.f);
+	return 0;
+} // btTranslationalLimitMotor::testLimitValue()
+
+//-----------------------------------------------------------------------------
+
 btScalar btTranslationalLimitMotor::solveLinearAxis(
-		btScalar timeStep,
-        btScalar jacDiagABInv,
-        btRigidBody& body1,const btVector3 &pointInA,
-        btRigidBody& body2,const btVector3 &pointInB,
-        int limit_index,
-        const btVector3 & axis_normal_on_a,
-		const btVector3 & anchorPos)
-{
-
-///find relative velocity
-//    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
-//    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
-    btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
-    btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
-
-    btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
-    btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
-    btVector3 vel = vel1 - vel2;
-
-    btScalar rel_vel = axis_normal_on_a.dot(vel);
-
-
-
-/// apply displacement correction
-
-//positional error (zeroth order error)
-    btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
-    btScalar	lo = btScalar(-1e30);
-    btScalar	hi = btScalar(1e30);
-
-    btScalar minLimit = m_lowerLimit[limit_index];
-    btScalar maxLimit = m_upperLimit[limit_index];
-
-    //handle the limits
-    if (minLimit < maxLimit)
-    {
-        {
-            if (depth > maxLimit)
-            {
-                depth -= maxLimit;
-                lo = btScalar(0.);
-
-            }
-            else
-            {
-                if (depth < minLimit)
-                {
-                    depth -= minLimit;
-                    hi = btScalar(0.);
-                }
-                else
-                {
-                    return 0.0f;
-                }
-            }
-        }
-    }
-
-    btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
-
-
-
-
-    btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
-    btScalar sum = oldNormalImpulse + normalImpulse;
-    m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
-    normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
-
-    btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
-    body1.applyImpulse( impulse_vector, rel_pos1);
-    body2.applyImpulse(-impulse_vector, rel_pos2);
-    return normalImpulse;
+	btScalar timeStep,
+	btScalar jacDiagABInv,
+	btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
+	btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
+	int limit_index,
+	const btVector3 & axis_normal_on_a,
+	const btVector3 & anchorPos)
+{
+
+	///find relative velocity
+	//    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
+	//    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
+	btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
+	btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
+
+	btVector3 vel1;
+	bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+	btVector3 vel2;
+	bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+	btVector3 vel = vel1 - vel2;
+
+	btScalar rel_vel = axis_normal_on_a.dot(vel);
+
+
+
+	/// apply displacement correction
+
+	//positional error (zeroth order error)
+	btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
+	btScalar	lo = btScalar(-1e30);
+	btScalar	hi = btScalar(1e30);
+
+	btScalar minLimit = m_lowerLimit[limit_index];
+	btScalar maxLimit = m_upperLimit[limit_index];
+
+	//handle the limits
+	if (minLimit < maxLimit)
+	{
+		{
+			if (depth > maxLimit)
+			{
+				depth -= maxLimit;
+				lo = btScalar(0.);
+
+			}
+			else
+			{
+				if (depth < minLimit)
+				{
+					depth -= minLimit;
+					hi = btScalar(0.);
+				}
+				else
+				{
+					return 0.0f;
+				}
+			}
+		}
+	}
+
+	btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
+
+
+
+
+	btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
+	btScalar sum = oldNormalImpulse + normalImpulse;
+	m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
+	normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
+
+	btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
+	//body1.applyImpulse( impulse_vector, rel_pos1);
+	//body2.applyImpulse(-impulse_vector, rel_pos2);
+
+	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);
+
+
+
+
+	return normalImpulse;
 }
 
 //////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
 
-
-btGeneric6DofConstraint::btGeneric6DofConstraint()
-        :btTypedConstraint(D6_CONSTRAINT_TYPE),
-		m_useLinearReferenceFrameA(true)
-{
-}
-
-btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
-        : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
-        , m_frameInA(frameInA)
-        , m_frameInB(frameInB),
-		m_useLinearReferenceFrameA(useLinearReferenceFrameA)
-{
-
-}
-
-
-
-
-
 void btGeneric6DofConstraint::calculateAngleInfo()
 {
 	btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
-
 	matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
-
-
-
 	// in euler angle mode we do not actually constrain the angular velocity
-  // along the axes axis[0] and axis[2] (although we do use axis[1]) :
-  //
-  //    to get			constrain w2-w1 along		...not
-  //    ------			---------------------		------
-  //    d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
-  //    d(angle[1])/dt = 0	ax[1]
-  //    d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
-  //
-  // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
-  // to prove the result for angle[0], write the expression for angle[0] from
-  // GetInfo1 then take the derivative. to prove this for angle[2] it is
-  // easier to take the euler rate expression for d(angle[2])/dt with respect
-  // to the components of w and set that to 0.
-
+	// along the axes axis[0] and axis[2] (although we do use axis[1]) :
+	//
+	//    to get			constrain w2-w1 along		...not
+	//    ------			---------------------		------
+	//    d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
+	//    d(angle[1])/dt = 0	ax[1]
+	//    d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
+	//
+	// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
+	// to prove the result for angle[0], write the expression for angle[0] from
+	// GetInfo1 then take the derivative. to prove this for angle[2] it is
+	// easier to take the euler rate expression for d(angle[2])/dt with respect
+	// to the components of w and set that to 0.
 	btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
 	btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
@@ -318,32 +367,27 @@
 	m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
 
-
-//    if(m_debugDrawer)
-//    {
-//
-//    	char buff[300];
-//		sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
-//		m_calculatedAxisAngleDiff[0],
-//		m_calculatedAxisAngleDiff[1],
-//		m_calculatedAxisAngleDiff[2]);
-//    	m_debugDrawer->reportErrorWarning(buff);
-//    }
-
-}
+	m_calculatedAxis[0].normalize();
+	m_calculatedAxis[1].normalize();
+	m_calculatedAxis[2].normalize();
+
+}
+
+//-----------------------------------------------------------------------------
 
 void btGeneric6DofConstraint::calculateTransforms()
 {
-    m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
-    m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
-
-    calculateAngleInfo();
-}
-
+	m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
+	m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
+	calculateLinearInfo();
+	calculateAngleInfo();
+}
+
+//-----------------------------------------------------------------------------
 
 void btGeneric6DofConstraint::buildLinearJacobian(
-    btJacobianEntry & jacLinear,const btVector3 & normalWorld,
-    const btVector3 & pivotAInW,const btVector3 & pivotBInW)
-{
-    new (&jacLinear) btJacobianEntry(
+	btJacobianEntry & jacLinear,const btVector3 & normalWorld,
+	const btVector3 & pivotAInW,const btVector3 & pivotBInW)
+{
+	new (&jacLinear) btJacobianEntry(
         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -355,11 +399,12 @@
         m_rbB.getInvInertiaDiagLocal(),
         m_rbB.getInvMass());
-
-}
+}
+
+//-----------------------------------------------------------------------------
 
 void btGeneric6DofConstraint::buildAngularJacobian(
-    btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
-{
-    new (&jacAngular)	btJacobianEntry(jointAxisW,
+	btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
+{
+	 new (&jacAngular)	btJacobianEntry(jointAxisW,
                                       m_rbA.getCenterOfMassTransform().getBasis().transpose(),
                                       m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -369,139 +414,257 @@
 }
 
+//-----------------------------------------------------------------------------
+
 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
 {
-    btScalar angle = m_calculatedAxisAngleDiff[axis_index];
-
-    //test limits
-    m_angularLimits[axis_index].testLimitValue(angle);
-    return m_angularLimits[axis_index].needApplyTorques();
-}
+	btScalar angle = m_calculatedAxisAngleDiff[axis_index];
+	//test limits
+	m_angularLimits[axis_index].testLimitValue(angle);
+	return m_angularLimits[axis_index].needApplyTorques();
+}
+
+//-----------------------------------------------------------------------------
 
 void btGeneric6DofConstraint::buildJacobian()
 {
-
-	// Clear accumulated impulses for the next simulation step
-    m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
-    int i;
-    for(i = 0; i < 3; i++)
-    {
-        m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
-    }
-    //calculates transform
-    calculateTransforms();
-
-//  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
-//  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
-	calcAnchorPos();
-	btVector3 pivotAInW = m_AnchorPos;
-	btVector3 pivotBInW = m_AnchorPos;
-
-// not used here
-//    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
-//    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-
-    btVector3 normalWorld;
-    //linear part
-    for (i=0;i<3;i++)
-    {
-        if (m_linearLimits.isLimited(i))
-        {
-			if (m_useLinearReferenceFrameA)
-	            normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
-			else
-	            normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
-
-            buildLinearJacobian(
-                m_jacLinear[i],normalWorld ,
-                pivotAInW,pivotBInW);
-
-        }
-    }
-
-    // angular part
-    for (i=0;i<3;i++)
-    {
-        //calculates error angle
-        if (testAngularLimitMotor(i))
-        {
-            normalWorld = this->getAxis(i);
-            // Create angular atom
-            buildAngularJacobian(m_jacAng[i],normalWorld);
-        }
-    }
-
-
-}
-
-
-void btGeneric6DofConstraint::solveConstraint(btScalar	timeStep)
-{
-    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,pointInA,
-                m_rbB,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,&m_rbB);
-        }
-    }
-}
+	if (m_useSolveConstraintObsolete)
+	{
+
+		// Clear accumulated impulses for the next simulation step
+		m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+		int i;
+		for(i = 0; i < 3; i++)
+		{
+			m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
+		}
+		//calculates transform
+		calculateTransforms();
+
+		//  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
+		//  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
+		calcAnchorPos();
+		btVector3 pivotAInW = m_AnchorPos;
+		btVector3 pivotBInW = m_AnchorPos;
+
+		// not used here
+		//    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
+		//    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+
+		btVector3 normalWorld;
+		//linear part
+		for (i=0;i<3;i++)
+		{
+			if (m_linearLimits.isLimited(i))
+			{
+				if (m_useLinearReferenceFrameA)
+					normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+				else
+					normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
+
+				buildLinearJacobian(
+					m_jacLinear[i],normalWorld ,
+					pivotAInW,pivotBInW);
+
+			}
+		}
+
+		// angular part
+		for (i=0;i<3;i++)
+		{
+			//calculates error angle
+			if (testAngularLimitMotor(i))
+			{
+				normalWorld = this->getAxis(i);
+				// Create angular atom
+				buildAngularJacobian(m_jacAng[i],normalWorld);
+			}
+		}
+
+	}
+}
+
+//-----------------------------------------------------------------------------
+
+void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
+	} else
+	{
+		//prepare constraint
+		calculateTransforms();
+		info->m_numConstraintRows = 0;
+		info->nub = 6;
+		int i;
+		//test linear limits
+		for(i = 0; i < 3; i++)
+		{
+			if(m_linearLimits.needApplyForce(i))
+			{
+				info->m_numConstraintRows++;
+				info->nub--;
+			}
+		}
+		//test angular limits
+		for (i=0;i<3 ;i++ )
+		{
+			if(testAngularLimitMotor(i))
+			{
+				info->m_numConstraintRows++;
+				info->nub--;
+			}
+		}
+	}
+}
+
+//-----------------------------------------------------------------------------
+
+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;
+	//solve linear limits
+	btRotationalLimitMotor limot;
+	for (int i=0;i<3 ;i++ )
+	{
+		if(m_linearLimits.needApplyForce(i))
+		{ // re-use rotational motor code
+			limot.m_bounce = btScalar(0.f);
+			limot.m_currentLimit = m_linearLimits.m_currentLimit[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;
+			limot.m_loLimit  = m_linearLimits.m_lowerLimit[i];
+			limot.m_maxLimitForce  = btScalar(0.f);
+			limot.m_maxMotorForce  = m_linearLimits.m_maxMotorForce[i];
+			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);
+		}
+	}
+	return row;
+}
+
+//-----------------------------------------------------------------------------
+
+int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset)
+{
+	btGeneric6DofConstraint * d6constraint = this;
+	int row = row_offset;
+	//solve angular limits
+	for (int i=0;i<3 ;i++ )
+	{
+		if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
+		{
+			btVector3 axis = d6constraint->getAxis(i);
+			row += get_limit_motor_info2(
+				d6constraint->getRotationalLimitMotor(i),
+				&m_rbA,
+				&m_rbB,
+				info,row,axis,1);
+		}
+	}
+
+	return row;
+}
+
+//-----------------------------------------------------------------------------
+
+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)
 {
-    (void)timeStep;
-
-}
+	(void)timeStep;
+
+}
+
+//-----------------------------------------------------------------------------
 
 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
 {
-    return m_calculatedAxis[axis_index];
-}
+	return m_calculatedAxis[axis_index];
+}
+
+//-----------------------------------------------------------------------------
 
 btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
 {
-    return m_calculatedAxisAngleDiff[axis_index];
-}
+	return m_calculatedAxisAngleDiff[axis_index];
+}
+
+//-----------------------------------------------------------------------------
 
 void btGeneric6DofConstraint::calcAnchorPos(void)
@@ -524,2 +687,143 @@
 } // btGeneric6DofConstraint::calcAnchorPos()
 
+//-----------------------------------------------------------------------------
+
+void btGeneric6DofConstraint::calculateLinearInfo()
+{
+	m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
+	m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
+	for(int i = 0; i < 3; 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)
+{
+    int srow = row * info->rowskip;
+    int powered = limot->m_enableMotor;
+    int limit = limot->m_currentLimit;
+    if (powered || limit)
+    {   // if the joint is powered, or has joint limits, add in the extra row
+        btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
+        btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
+        J1[srow+0] = ax1[0];
+        J1[srow+1] = ax1[1];
+        J1[srow+2] = ax1[2];
+        if(rotational)
+        {
+            J2[srow+0] = -ax1[0];
+            J2[srow+1] = -ax1[1];
+            J2[srow+2] = -ax1[2];
+        }
+        if((!rotational) && limit)
+        {
+			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 we're limited low and high simultaneously, the joint motor is
+        // ineffective
+        if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
+        info->m_constraintError[srow] = btScalar(0.f);
+        if (powered)
+        {
+            info->cfm[srow] = 0.0f;
+            if(!limit)
+            {
+                info->m_constraintError[srow] += limot->m_targetVelocity;
+                info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
+                info->m_upperLimit[srow] = limot->m_maxMotorForce;
+            }
+        }
+        if(limit)
+        {
+            btScalar k = info->fps * limot->m_ERP;
+			if(!rotational)
+			{
+				info->m_constraintError[srow] += k * limot->m_currentLimitError;
+			}
+			else
+			{
+				info->m_constraintError[srow] += -k * limot->m_currentLimitError;
+			}
+            info->cfm[srow] = 0.0f;
+            if (limot->m_loLimit == limot->m_hiLimit)
+            {   // limited low and high simultaneously
+                info->m_lowerLimit[srow] = -SIMD_INFINITY;
+                info->m_upperLimit[srow] = SIMD_INFINITY;
+            }
+            else
+            {
+                if (limit == 1)
+                {
+                    info->m_lowerLimit[srow] = 0;
+                    info->m_upperLimit[srow] = SIMD_INFINITY;
+                }
+                else
+                {
+                    info->m_lowerLimit[srow] = -SIMD_INFINITY;
+                    info->m_upperLimit[srow] = 0;
+                }
+                // deal with bounce
+                if (limot->m_bounce > 0)
+                {
+                    // calculate joint velocity
+                    btScalar vel;
+                    if (rotational)
+                    {
+                        vel = body0->getAngularVelocity().dot(ax1);
+                        if (body1)
+                            vel -= body1->getAngularVelocity().dot(ax1);
+                    }
+                    else
+                    {
+                        vel = body0->getLinearVelocity().dot(ax1);
+                        if (body1)
+                            vel -= body1->getLinearVelocity().dot(ax1);
+                    }
+                    // only apply bounce if the velocity is incoming, and if the
+                    // resulting c[] exceeds what we already have.
+                    if (limit == 1)
+                    {
+                        if (vel < 0)
+                        {
+                            btScalar newc = -limot->m_bounce* vel;
+                            if (newc > info->m_constraintError[srow]) 
+								info->m_constraintError[srow] = newc;
+                        }
+                    }
+                    else
+                    {
+                        if (vel > 0)
+                        {
+                            btScalar newc = -limot->m_bounce * vel;
+                            if (newc < info->m_constraintError[srow]) 
+								info->m_constraintError[srow] = newc;
+                        }
+                    }
+                }
+            }
+        }
+        return 1;
+    }
+    else return 0;
+}
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h	(revision 2907)
@@ -29,4 +29,6 @@
 
 class btRigidBody;
+
+
 
 
@@ -93,5 +95,5 @@
     bool isLimited()
     {
-    	if(m_loLimit>=m_hiLimit) return false;
+    	if(m_loLimit > m_hiLimit) return false;
     	return true;
     }
@@ -111,6 +113,5 @@
 
 	//! apply the correction impulses for two bodies
-    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
-
+    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
 
 };
@@ -130,4 +131,9 @@
     btScalar	m_restitution;//! Bounce parameter for linear limit
     //!@}
+	bool		m_enableMotor[3];
+    btVector3	m_targetVelocity;//!< target motor velocity
+    btVector3	m_maxMotorForce;//!< max force on motor
+    btVector3	m_currentLimitError;//!  How much is violated this limit
+    int			m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
 
     btTranslationalLimitMotor()
@@ -140,4 +146,10 @@
     	m_damping = btScalar(1.0f);
     	m_restitution = btScalar(0.5f);
+		for(int i=0; i < 3; i++) 
+		{
+			m_enableMotor[i] = false;
+			m_targetVelocity[i] = btScalar(0.f);
+			m_maxMotorForce[i] = btScalar(0.f);
+		}
     }
 
@@ -151,4 +163,10 @@
     	m_damping = other.m_damping;
     	m_restitution = other.m_restitution;
+		for(int i=0; i < 3; i++) 
+		{
+			m_enableMotor[i] = other.m_enableMotor[i];
+			m_targetVelocity[i] = other.m_targetVelocity[i];
+			m_maxMotorForce[i] = other.m_maxMotorForce[i];
+		}
     }
 
@@ -164,4 +182,10 @@
        return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
     }
+    inline bool needApplyForce(int limitIndex)
+    {
+    	if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
+    	return true;
+    }
+	int testLimitValue(int limitIndex, btScalar test_value);
 
 
@@ -169,6 +193,6 @@
     	btScalar timeStep,
         btScalar jacDiagABInv,
-        btRigidBody& body1,const btVector3 &pointInA,
-        btRigidBody& body2,const btVector3 &pointInB,
+        btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
+        btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
         int limit_index,
         const btVector3 & axis_normal_on_a,
@@ -248,4 +272,5 @@
     btVector3 m_calculatedAxisAngleDiff;
     btVector3 m_calculatedAxis[3];
+    btVector3 m_calculatedLinearDiff;
     
 	btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
@@ -263,4 +288,7 @@
 
 
+	int setAngularLimits(btConstraintInfo2 *info, int row_offset);
+
+	int setLinearLimits(btConstraintInfo2 *info);
 
     void buildLinearJacobian(
@@ -270,4 +298,6 @@
     void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
 
+	// tests linear limits
+	void calculateLinearInfo();
 
 	//! calcs the euler angles between the two bodies.
@@ -277,4 +307,8 @@
 
 public:
+
+	///for backwards compatibility during the transition to 'getInfo/getInfo2'
+	bool		m_useSolveConstraintObsolete;
+
     btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
 
@@ -331,5 +365,9 @@
     virtual void	buildJacobian();
 
-    virtual	void	solveConstraint(btScalar	timeStep);
+	virtual void getInfo1 (btConstraintInfo1* info);
+
+	virtual void getInfo2 (btConstraintInfo2* info);
+
+    virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
 
     void	updateRHS(btScalar	timeStep);
@@ -433,4 +471,9 @@
 	virtual void calcAnchorPos(void); // overridable
 
+	int get_limit_motor_info2(	btRotationalLimitMotor * limot,
+								btRigidBody * body0, btRigidBody * body1,
+								btConstraintInfo2 *info, int row, btVector3& ax1, int rotational);
+
+
 };
 
Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp	(revision 2907)
@@ -20,17 +20,31 @@
 #include "LinearMath/btMinMax.h"
 #include <new>
+#include "btSolverBody.h"
+
+//-----------------------------------------------------------------------------
+
+#define HINGE_USE_OBSOLETE_SOLVER false
+
+//-----------------------------------------------------------------------------
 
 
 btHingeConstraint::btHingeConstraint()
 : btTypedConstraint (HINGE_CONSTRAINT_TYPE),
-m_enableAngularMotor(false)
-{
-}
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(false)
+{
+	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+
+//-----------------------------------------------------------------------------
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
-									 btVector3& axisInA,btVector3& axisInB)
+									 btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
 									 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
 									 m_angularOnly(false),
-									 m_enableAngularMotor(false)
+									 m_enableAngularMotor(false),
+									 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+									 m_useReferenceFrameA(useReferenceFrameA)
 {
 	m_rbAFrame.getOrigin() = pivotInA;
@@ -61,7 +75,7 @@
 	
 	m_rbBFrame.getOrigin() = pivotInB;
-	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
-									rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
-									rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
+	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
+									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
+									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
 	
 	//start with free
@@ -72,11 +86,13 @@
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
-
-}
-
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
-:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
+	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+
+//-----------------------------------------------------------------------------
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), 
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
 {
 
@@ -91,5 +107,5 @@
 									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
 
-	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
+	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
 
 	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
@@ -110,17 +126,17 @@
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
-}
+	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+
+//-----------------------------------------------------------------------------
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
-								     const btTransform& rbAFrame, const btTransform& rbBFrame)
+								     const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
 m_angularOnly(false),
-m_enableAngularMotor(false)
-{
-	// flip axis
-	m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
-
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
+{
 	//start with free
 	m_lowerLimit = btScalar(1e30);
@@ -130,19 +146,17 @@
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
+	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }			
 
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
+//-----------------------------------------------------------------------------
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
 m_angularOnly(false),
-m_enableAngularMotor(false)
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
 {
 	///not providing rigidbody B means implicitly using worldspace for body B
-
-	// flip axis
-	m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
 
 	m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
@@ -155,31 +169,36 @@
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
-}
+	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+
+//-----------------------------------------------------------------------------
 
 void	btHingeConstraint::buildJacobian()
 {
-	m_appliedImpulse = btScalar(0.);
-
-	if (!m_angularOnly)
-	{
-		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
-		btVector3 relPos = pivotBInW - pivotAInW;
-
-		btVector3 normal[3];
-		if (relPos.length2() > SIMD_EPSILON)
-		{
-			normal[0] = relPos.normalized();
-		}
-		else
-		{
-			normal[0].setValue(btScalar(1.0),0,0);
-		}
-
-		btPlaneSpace1(normal[0], normal[1], normal[2]);
-
-		for (int i=0;i<3;i++)
-		{
-			new (&m_jac[i]) btJacobianEntry(
+	if (m_useSolveConstraintObsolete)
+	{
+		m_appliedImpulse = btScalar(0.);
+
+		if (!m_angularOnly)
+		{
+			btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+			btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+			btVector3 relPos = pivotBInW - pivotAInW;
+
+			btVector3 normal[3];
+			if (relPos.length2() > SIMD_EPSILON)
+			{
+				normal[0] = relPos.normalized();
+			}
+			else
+			{
+				normal[0].setValue(btScalar(1.0),0,0);
+			}
+
+			btPlaneSpace1(normal[0], normal[1], normal[2]);
+
+			for (int i=0;i<3;i++)
+			{
+				new (&m_jac[i]) btJacobianEntry(
 				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
 				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -191,212 +210,457 @@
 				m_rbB.getInvInertiaDiagLocal(),
 				m_rbB.getInvMass());
+			}
 		}
-	}
-
-	//calculate two perpendicular jointAxis, orthogonal to hingeAxis
-	//these two jointAxis require equal angular velocities for both bodies
-
-	//this is unused for now, it's a todo
-	btVector3 jointAxis0local;
-	btVector3 jointAxis1local;
-	
-	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;
-	btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+
+		//calculate two perpendicular jointAxis, orthogonal to hingeAxis
+		//these two jointAxis require equal angular velocities for both bodies
+
+		//this is unused for now, it's a todo
+		btVector3 jointAxis0local;
+		btVector3 jointAxis1local;
 		
-	new (&m_jacAng[0])	btJacobianEntry(jointAxis0,
-		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-		m_rbA.getInvInertiaDiagLocal(),
-		m_rbB.getInvInertiaDiagLocal());
-
-	new (&m_jacAng[1])	btJacobianEntry(jointAxis1,
-		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-		m_rbA.getInvInertiaDiagLocal(),
-		m_rbB.getInvInertiaDiagLocal());
-
-	new (&m_jacAng[2])	btJacobianEntry(hingeAxisWorld,
-		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-		m_rbA.getInvInertiaDiagLocal(),
-		m_rbB.getInvInertiaDiagLocal());
-
-
+		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;
+		btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+			
+		new (&m_jacAng[0])	btJacobianEntry(jointAxis0,
+			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+			m_rbA.getInvInertiaDiagLocal(),
+			m_rbB.getInvInertiaDiagLocal());
+
+		new (&m_jacAng[1])	btJacobianEntry(jointAxis1,
+			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+			m_rbA.getInvInertiaDiagLocal(),
+			m_rbB.getInvInertiaDiagLocal());
+
+		new (&m_jacAng[2])	btJacobianEntry(hingeAxisWorld,
+			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+			m_rbA.getInvInertiaDiagLocal(),
+			m_rbB.getInvInertiaDiagLocal());
+
+			// clear accumulator
+			m_accLimitImpulse = btScalar(0.);
+
+			// test angular limit
+			testLimit();
+
+		//Compute K = J*W*J' for hinge axis
+		btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
+		m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
+							 getRigidBodyB().computeAngularImpulseDenominator(axisA));
+
+	}
+}
+
+//-----------------------------------------------------------------------------
+
+void btHingeConstraint::getInfo1(btConstraintInfo1* info)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
+	}
+	else
+	{
+		info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
+		info->nub = 1; 
+		//prepare constraint
+		testLimit();
+		if(getSolveLimit() || getEnableAngularMotor())
+		{
+			info->m_numConstraintRows++; // limit 3rd anguar as well
+			info->nub--; 
+		}
+	}
+} // btHingeConstraint::getInfo1 ()
+
+//-----------------------------------------------------------------------------
+
+void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	btAssert(!m_useSolveConstraintObsolete);
+	int i, s = info->rowskip;
+	// transforms in world space
+	btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
+	btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
+	// pivot point
+	btVector3 pivotAInW = trA.getOrigin();
+	btVector3 pivotBInW = trB.getOrigin();
+	// 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();
+	{
+		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s);
+		btVector3 a1neg = -a1;
+		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+	btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
+	{
+		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s);
+		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]);
+    }
+	// make rotations around X and Y equal
+	// 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.
+	// get hinge axis (Z)
+	btVector3 ax1 = trA.getBasis().getColumn(2);
+	// get 2 orthos to hinge axis (X, Y)
+	btVector3 p = trA.getBasis().getColumn(0);
+	btVector3 q = trA.getBasis().getColumn(1);
+	// set the two hinge angular rows 
+    int s3 = 3 * info->rowskip;
+    int s4 = 4 * info->rowskip;
+
+	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 ax1,ax2 are the unit length hinge axes as computed from body1 and
+    // body2, 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.
+    btVector3 ax2 = trB.getBasis().getColumn(2);
+	btVector3 u = ax1.cross(ax2);
+	info->m_constraintError[s3] = k * u.dot(p);
+	info->m_constraintError[s4] = k * u.dot(q);
+	// check angular limits
+	int 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);
+		if(powered)
+		{
+            info->cfm[srow] = btScalar(0.0); 
+			btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
+			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 * info->erp;
+			info->m_constraintError[srow] += k * limit_err;
+			info->cfm[srow] = btScalar(0.0);
+			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 = m_rbA.getAngularVelocity().dot(ax1);
+				vel -= m_rbB.getAngularVelocity().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
+}
+
+//-----------------------------------------------------------------------------
+
+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)
+{
+	(void)timeStep;
+
+}
+
+//-----------------------------------------------------------------------------
+
+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 m_referenceSign * angle;
+}
+
+//-----------------------------------------------------------------------------
+
+void btHingeConstraint::testLimit()
+{
 	// Compute limit information
-	btScalar hingeAngle = getHingeAngle();  
-
-	//set bias, sign, clear accumulator
+	m_hingeAngle = getHingeAngle();  
 	m_correction = btScalar(0.);
 	m_limitSign = btScalar(0.);
 	m_solveLimit = false;
-	m_accLimitImpulse = btScalar(0.);
-
-//	if (m_lowerLimit < m_upperLimit)
 	if (m_lowerLimit <= m_upperLimit)
 	{
-//		if (hingeAngle <= m_lowerLimit*m_limitSoftness)
-		if (hingeAngle <= m_lowerLimit)
-		{
-			m_correction = (m_lowerLimit - hingeAngle);
+		if (m_hingeAngle <= m_lowerLimit)
+		{
+			m_correction = (m_lowerLimit - m_hingeAngle);
 			m_limitSign = 1.0f;
 			m_solveLimit = true;
 		} 
-//		else if (hingeAngle >= m_upperLimit*m_limitSoftness)
-		else if (hingeAngle >= m_upperLimit)
-		{
-			m_correction = m_upperLimit - hingeAngle;
+		else if (m_hingeAngle >= m_upperLimit)
+		{
+			m_correction = m_upperLimit - m_hingeAngle;
 			m_limitSign = -1.0f;
 			m_solveLimit = true;
 		}
 	}
-
-	//Compute K = J*W*J' for hinge axis
-	btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
-	m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
-			             getRigidBodyB().computeAngularImpulseDenominator(axisA));
-
-}
-
-void	btHingeConstraint::solveConstraint(btScalar	timeStep)
-{
-
-	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 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-		btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-		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;
-			m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
-			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
-		}
-	}
-
-	
-	{
-		///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);
-
-		const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
-		const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
-
-		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;
-			}
-
-			//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;
-			}
-
-			m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
-			m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
-
-			// 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;
-
-
-				btVector3 impulse = axisA * impulseMag * m_limitSign;
-				m_rbA.applyTorqueImpulse(impulse);
-				m_rbB.applyTorqueImpulse(-impulse);
-			}
-		}
-
-		//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;
-
-			m_rbA.applyTorqueImpulse(motorImp+angularLimit);
-			m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
-			
-		}
-	}
-
-}
-
-void	btHingeConstraint::updateRHS(btScalar	timeStep)
-{
-	(void)timeStep;
-
-}
-
-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);
-
-	return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
-}
-
+	return;
+} // btHingeConstraint::testLimit()
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h	(revision 2907)
@@ -54,19 +54,23 @@
 
 	btScalar	m_accLimitImpulse;
+	btScalar	m_hingeAngle;
+	btScalar    m_referenceSign;
 
 	bool		m_angularOnly;
 	bool		m_enableAngularMotor;
 	bool		m_solveLimit;
+	bool		m_useSolveConstraintObsolete;
+	bool		m_useReferenceFrameA;
 
 	
 public:
 
-	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB);
+	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);
+	btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false);
 	
-	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
+	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
 
-	btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
+	btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
 
 	btHingeConstraint();
@@ -74,5 +78,9 @@
 	virtual void	buildJacobian();
 
-	virtual	void	solveConstraint(btScalar	timeStep);
+	virtual void getInfo1 (btConstraintInfo1* info);
+
+	virtual void getInfo2 (btConstraintInfo2* info);
+	
+	virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
 
 	void	updateRHS(btScalar	timeStep);
@@ -87,4 +95,14 @@
 	}
 
+	btRigidBody& getRigidBodyA()	
+	{		
+		return m_rbA;	
+	}	
+
+	btRigidBody& getRigidBodyB()	
+	{		
+		return m_rbB;	
+	}	
+	
 	void	setAngularOnly(bool angularOnly)
 	{
@@ -123,4 +141,6 @@
 	btScalar getHingeAngle();
 
+	void testLimit();
+
 
 	const btTransform& getAFrame() { return m_rbAFrame; };	
Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp	(revision 2907)
@@ -22,10 +22,12 @@
 
 btPoint2PointConstraint::btPoint2PointConstraint()
-:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE)
+: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)
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
+m_useSolveConstraintObsolete(false)
 {
 
@@ -34,5 +36,6 @@
 
 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
-:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
+m_useSolveConstraintObsolete(false)
 {
 	
@@ -41,12 +44,14 @@
 void	btPoint2PointConstraint::buildJacobian()
 {
-	m_appliedImpulse = btScalar(0.);
-
-	btVector3	normal(0,0,0);
-
-	for (int i=0;i<3;i++)
-	{
-		normal[i] = 1;
-		new (&m_jac[i]) btJacobianEntry(
+	///we need it for both methods
+	{
+		m_appliedImpulse = btScalar(0.);
+
+		btVector3	normal(0,0,0);
+
+		for (int i=0;i<3;i++)
+		{
+			normal[i] = 1;
+			new (&m_jac[i]) btJacobianEntry(
 			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
 			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -59,62 +64,160 @@
 			m_rbB.getInvMass());
 		normal[i] = 0;
-	}
-
-}
-
-void	btPoint2PointConstraint::solveConstraint(btScalar	timeStep)
-{
-	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 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-		btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-		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);
+		}
+	}
+
+}
+
+
+void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
+	} else
+	{
+		info->m_numConstraintRows = 3;
+		info->nub = 3;
+	}
+}
+
+void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	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.
+   
+    // 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()*getPivotInA();
+	{
+		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+		btVector3 a1neg = -a1;
+		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+    
+	/*info->m_J2linearAxis[0] = -1;
+    info->m_J2linearAxis[s+1] = -1;
+    info->m_J2linearAxis[2*s+2] = -1;
 	*/
 	
-		//positional error (zeroth order error)
-		btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+	btVector3 a2 = body1_trans.getBasis()*getPivotInB();
+   
+	{
+		btVector3 a2n = -a2;
+		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+    
+
+
+    // set right hand side
+    btScalar k = info->fps * info->erp;
+    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]);
+		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
+    }
+
+	btScalar impulseClamp = m_setting.m_impulseClamp;//
+	for (j=0; j<3; j++)
+    {
+		if (m_setting.m_impulseClamp > 0)
+		{
+			info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
+			info->m_upperLimit[j*info->rowskip] = impulseClamp;
+		}
+	}
+	
+}
+
+
+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);
 		
-		btScalar impulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
-
-		btScalar impulseClamp = m_setting.m_impulseClamp;
-		if (impulseClamp > 0)
-		{
-			if (impulse < -impulseClamp) 
-				impulse = -impulseClamp;
-			if (impulse > impulseClamp) 
-				impulse = impulseClamp;
+
+	//	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;
 		}
-
-		m_appliedImpulse+=impulse;
-		btVector3 impulse_vector = normal * impulse;
-		m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
-		m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
-		
-		normal[i] = 0;
 	}
 }
Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h	(revision 2907)
@@ -51,4 +51,7 @@
 public:
 
+	///for backwards compatibility during the transition to 'getInfo/getInfo2'
+	bool		m_useSolveConstraintObsolete;
+
 	btConstraintSetting	m_setting;
 
@@ -61,6 +64,10 @@
 	virtual void	buildJacobian();
 
+	virtual void getInfo1 (btConstraintInfo1* info);
 
-	virtual	void	solveConstraint(btScalar	timeStep);
+	virtual void getInfo2 (btConstraintInfo2* info);
+
+
+	virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
 
 	void	updateRHS(btScalar	timeStep);
Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp	(revision 2907)
@@ -16,5 +16,4 @@
 //#define COMPUTE_IMPULSE_DENOM 1
 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
-//#define FORCE_REFESH_CONTACT_MANIFOLDS 1
 
 #include "btSequentialImpulseConstraintSolver.h"
@@ -33,29 +32,154 @@
 #include "btSolverBody.h"
 #include "btSolverConstraint.h"
-
-
 #include "LinearMath/btAlignedObjectArray.h"
-
-
-int totalCpd = 0;
-
-int	gTotalContactPoints = 0;
-
-struct	btOrderIndex
-{
-	int	m_manifoldIndex;
-	int	m_pointIndex;
-};
-
-
-
-#define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
-static btOrderIndex	gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
+#include <string.h> //for memset
+
+btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
+:m_btSeed2(0)
+{
+
+}
+
+btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
+{
+}
+
+#ifdef USE_SIMD
+#include <emmintrin.h>
+#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
+static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 )
+{
+	__m128 result = _mm_mul_ps( vec0, vec1);
+	return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) );
+}
+#endif//USE_SIMD
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+#ifdef USE_SIMD
+	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
+	__m128	lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+	__m128	upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
+	__m128 deltaVel1Dotn	=	_mm_add_ps(_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));
+	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 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
+	deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
+	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
+	__m128	linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
+	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
+	__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));
+#else
+	resolveSingleConstraintRowGeneric(body1,body2,c);
+#endif
+}
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
+	const btScalar deltaVel1Dotn	=	c.m_contactNormal.dot(body1.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;
+	deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
+	deltaImpulse	-=	deltaVel2Dotn*c.m_jacDiagABInv;
+
+	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
+	if (sum < c.m_lowerLimit)
+	{
+		deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
+		c.m_appliedImpulse = c.m_lowerLimit;
+	}
+	else if (sum > c.m_upperLimit) 
+	{
+		deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
+		c.m_appliedImpulse = c.m_upperLimit;
+	}
+	else
+	{
+		c.m_appliedImpulse = sum;
+	}
+	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)
+{
+#ifdef USE_SIMD
+	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
+	__m128	lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+	__m128	upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
+	__m128 deltaVel1Dotn	=	_mm_add_ps(_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));
+	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,_mm_set1_ps(body1.m_invMass));
+	__m128	linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
+	__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));
+#else
+	resolveSingleConstraintRowLowerLimit(body1,body2,c);
+#endif
+}
+
+// Project Gauss Seidel or the equivalent Sequential Impulse
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
+	const btScalar deltaVel1Dotn	=	c.m_contactNormal.dot(body1.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);
+
+	deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
+	deltaImpulse	-=	deltaVel2Dotn*c.m_jacDiagABInv;
+	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
+	if (sum < c.m_lowerLimit)
+	{
+		deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
+		c.m_appliedImpulse = c.m_lowerLimit;
+	}
+	else
+	{
+		c.m_appliedImpulse = sum;
+	}
+	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);
+}
+
 
 
 unsigned long btSequentialImpulseConstraintSolver::btRand2()
 {
-  m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
-  return m_btSeed2;
+	m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
+	return m_btSeed2;
 }
 
@@ -65,91 +189,49 @@
 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
 {
-  // seems good; xor-fold and modulus
-  const unsigned long un = static_cast<unsigned long>(n);
-  unsigned long r = btRand2();
-
-  // note: probably more aggressive than it needs to be -- might be
-  //       able to get away without one or two of the innermost branches.
-  if (un <= 0x00010000UL) {
-    r ^= (r >> 16);
-    if (un <= 0x00000100UL) {
-      r ^= (r >> 8);
-      if (un <= 0x00000010UL) {
-        r ^= (r >> 4);
-        if (un <= 0x00000004UL) {
-          r ^= (r >> 2);
-          if (un <= 0x00000002UL) {
-            r ^= (r >> 1);
-          }
-        }
-     }
-    }
-   }
-
-  return (int) (r % un);
-}
-
-
-
-
-bool  MyContactDestroyedCallback(void* userPersistentData);
-bool  MyContactDestroyedCallback(void* userPersistentData)
-{
-	assert (userPersistentData);
-	btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData;
-	btAlignedFree(cpd);
-	totalCpd--;
-	//printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
-	return true;
-}
-
-
-
-btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
-:m_btSeed2(0)
-{
-	gContactDestroyedCallback = &MyContactDestroyedCallback;
-
-	//initialize default friction/contact funcs
-	int i,j;
-	for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++)
-		for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++)
-		{
-
-			m_contactDispatch[i][j] = resolveSingleCollision;
-			m_frictionDispatch[i][j] = resolveSingleFriction;
-		}
-}
-
-btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
-{
-
-}
-
-void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
-void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
-{
-	btRigidBody* rb = btRigidBody::upcast(collisionObject);
+	// seems good; xor-fold and modulus
+	const unsigned long un = static_cast<unsigned long>(n);
+	unsigned long r = btRand2();
+
+	// note: probably more aggressive than it needs to be -- might be
+	//       able to get away without one or two of the innermost branches.
+	if (un <= 0x00010000UL) {
+		r ^= (r >> 16);
+		if (un <= 0x00000100UL) {
+			r ^= (r >> 8);
+			if (un <= 0x00000010UL) {
+				r ^= (r >> 4);
+				if (un <= 0x00000004UL) {
+					r ^= (r >> 2);
+					if (un <= 0x00000002UL) {
+						r ^= (r >> 1);
+					}
+				}
+			}
+		}
+	}
+
+	return (int) (r % un);
+}
+
+
+
+void	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);
+
 	if (rb)
 	{
-		solverBody->m_angularVelocity = rb->getAngularVelocity() ;
-		solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
-		solverBody->m_friction = collisionObject->getFriction();
 		solverBody->m_invMass = rb->getInvMass();
-		solverBody->m_linearVelocity = rb->getLinearVelocity();
 		solverBody->m_originalBody = rb;
 		solverBody->m_angularFactor = rb->getAngularFactor();
 	} else
 	{
-		solverBody->m_angularVelocity.setValue(0,0,0);
-		solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
-		solverBody->m_friction = collisionObject->getFriction();
 		solverBody->m_invMass = 0.f;
-		solverBody->m_linearVelocity.setValue(0,0,0);
 		solverBody->m_originalBody = 0;
 		solverBody->m_angularFactor = 1.f;
 	}
-	solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
-	solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
 }
 
@@ -157,6 +239,5 @@
 int		gNumSplitImpulseRecoveries = 0;
 
-btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
-btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
+btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
 {
 	btScalar rest = restitution * -rel_vel;
@@ -165,274 +246,19 @@
 
 
-void	resolveSplitPenetrationImpulseCacheFriendly(
-        btSolverBody& body1,
-        btSolverBody& body2,
-        const btSolverConstraint& contactConstraint,
-        const btContactSolverInfo& solverInfo);
-
-//SIMD_FORCE_INLINE
-void	resolveSplitPenetrationImpulseCacheFriendly(
-        btSolverBody& body1,
-        btSolverBody& body2,
-        const btSolverConstraint& contactConstraint,
-        const btContactSolverInfo& solverInfo)
-{
-        (void)solverInfo;
-
-		if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold)
-        {
-
-				gNumSplitImpulseRecoveries++;
-                btScalar normalImpulse;
-
-                //  Optimized version of projected relative velocity, use precomputed cross products with normal
-                //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
-                //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
-                //      btVector3 vel = vel1 - vel2;
-                //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
-
-                btScalar rel_vel;
-                btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity)
-                + contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity);
-                btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity)
-                + contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity);
-
-                rel_vel = vel1Dotn-vel2Dotn;
-
-
-				btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep;
-                //      btScalar positionalError = contactConstraint.m_penetration;
-
-                btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
-
-                btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
-                btScalar        velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
-                normalImpulse = penetrationImpulse+velocityImpulse;
-
-                // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-                btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse;
-                btScalar sum = oldNormalImpulse + normalImpulse;
-                contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-
-                normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse;
-
-				body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse);
-               
-				body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse);
-               
-        }
-
-}
-
-
-//velocity + friction
-//response  between two dynamic objects with friction
-
-btScalar resolveSingleCollisionCombinedCacheFriendly(
-	btSolverBody& body1,
-	btSolverBody& body2,
-	const btSolverConstraint& contactConstraint,
-	const btContactSolverInfo& solverInfo);
-
-//SIMD_FORCE_INLINE 
-btScalar resolveSingleCollisionCombinedCacheFriendly(
-	btSolverBody& body1,
-	btSolverBody& body2,
-	const btSolverConstraint& contactConstraint,
-	const btContactSolverInfo& solverInfo)
-{
-	(void)solverInfo;
-
-	btScalar normalImpulse;
-
-	{
-
-		
-		//  Optimized version of projected relative velocity, use precomputed cross products with normal
-		//	body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
-		//	body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
-		//	btVector3 vel = vel1 - vel2;
-		//	btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
-
-		btScalar rel_vel;
-		btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
-					+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
-		btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
-					+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
-
-		rel_vel = vel1Dotn-vel2Dotn;
-
-		btScalar positionalError = 0.f;
-		if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold))
-		{
- 			positionalError = -contactConstraint.m_penetration * solverInfo.m_erp/solverInfo.m_timeStep;
-		}
-
-		btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
-
-		btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
-		btScalar	velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
-		normalImpulse = penetrationImpulse+velocityImpulse;
-		
-		
-		// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-		btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
-		btScalar sum = oldNormalImpulse + normalImpulse;
-		contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-
-		normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
-
-		body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
-				contactConstraint.m_angularComponentA,normalImpulse);
-		
-		body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
-				contactConstraint.m_angularComponentB,-normalImpulse);
-	}
-
-	return normalImpulse;
-}
-
-
-#ifndef NO_FRICTION_TANGENTIALS
-
-btScalar resolveSingleFrictionCacheFriendly(
-	btSolverBody& body1,
-	btSolverBody& body2,
-	const btSolverConstraint& contactConstraint,
-	const btContactSolverInfo& solverInfo,
-	btScalar appliedNormalImpulse);
-
-//SIMD_FORCE_INLINE 
-btScalar resolveSingleFrictionCacheFriendly(
-	btSolverBody& body1,
-	btSolverBody& body2,
-	const btSolverConstraint& contactConstraint,
-	const btContactSolverInfo& solverInfo,
-	btScalar appliedNormalImpulse)
-{
-	(void)solverInfo;
-
-	
-	const btScalar combinedFriction = contactConstraint.m_friction;
-	
-	const btScalar limit = appliedNormalImpulse * combinedFriction;
-	
-	if (appliedNormalImpulse>btScalar(0.))
-	//friction
-	{
-		
-		btScalar j1;
-		{
-
-			btScalar rel_vel;
-			const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
-						+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
-			const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
-				+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
-			rel_vel = vel1Dotn-vel2Dotn;
-
-			// calculate j that moves us to zero relative velocity
-			j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
-#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
-#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
-			btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
-			contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
-			
-			if (limit < contactConstraint.m_appliedImpulse)
-			{
-				contactConstraint.m_appliedImpulse = limit;
-			} else
-			{
-				if (contactConstraint.m_appliedImpulse < -limit)
-					contactConstraint.m_appliedImpulse = -limit;
-			}
-			j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
-#else
-			if (limit < j1)
-			{
-				j1 = limit;
-			} else
-			{
-				if (j1 < -limit)
-					j1 = -limit;
-			}
-
-#endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE
-
-			//GEN_set_min(contactConstraint.m_appliedImpulse, limit);
-			//GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
-
-			
-
-		}
-	
-		body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
-		
-		body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
-
-	} 
-	return 0.f;
-}
-
-
-#else
-
-//velocity + friction
-//response  between two dynamic objects with friction
-btScalar resolveSingleFrictionCacheFriendly(
-	btSolverBody& body1,
-	btSolverBody& body2,
-	btSolverConstraint& contactConstraint,
-	const btContactSolverInfo& solverInfo)
-{
-
-	btVector3 vel1;
-	btVector3 vel2;
-	btScalar normalImpulse(0.f);
-
-	{
-		const btVector3& normal = contactConstraint.m_contactNormal;
-		if (contactConstraint.m_penetration < 0.f)
-			return 0.f;
-
-
-		body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
-		body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
-		btVector3 vel = vel1 - vel2;
-		btScalar rel_vel;
-		rel_vel = normal.dot(vel);
-
-		btVector3 lat_vel = vel - normal * rel_vel;
-		btScalar lat_rel_vel = lat_vel.length2();
-
-		btScalar combinedFriction = contactConstraint.m_friction;
-		const btVector3& rel_pos1 = contactConstraint.m_rel_posA;
-		const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
-
-
-		if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
-		{
-			lat_rel_vel = btSqrt(lat_rel_vel);
-
-			lat_vel /= lat_rel_vel;
-			btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel);
-			btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
-			btScalar friction_impulse = lat_rel_vel /
-				(body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
-			btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction;
-
-			GEN_set_min(friction_impulse, normal_impulse);
-			GEN_set_max(friction_impulse, -normal_impulse);
-			body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
-			body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
-		}
-	}
-
-	return normalImpulse;
-}
-
-#endif //NO_FRICTION_TANGENTIALS
-
-
+
+void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
+void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
+{
+	if (colObj && colObj->hasAnisotropicFriction())
+	{
+		// transform to local coordinates
+		btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
+		const btVector3& friction_scaling = colObj->getAnisotropicFriction();
+		//apply anisotropic friction
+		loc_lateral *= friction_scaling;
+		// ... and transform it back to global coordinates
+		frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
+	}
+}
 
 
@@ -440,14 +266,15 @@
 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)
 {
+
 
 	btRigidBody* body0=btRigidBody::upcast(colObj0);
 	btRigidBody* body1=btRigidBody::upcast(colObj1);
 
-	btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
+	btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
+	memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
 	solverConstraint.m_contactNormal = normalAxis;
 
 	solverConstraint.m_solverBodyIdA = solverBodyIdA;
 	solverConstraint.m_solverBodyIdB = solverBodyIdB;
-	solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
 	solverConstraint.m_frictionIndex = frictionIndex;
 
@@ -455,16 +282,16 @@
 	solverConstraint.m_originalContactPoint = 0;
 
-	solverConstraint.m_appliedImpulse = btScalar(0.);
-	solverConstraint.m_appliedPushImpulse = 0.f;
-	solverConstraint.m_penetration = 0.f;
+	solverConstraint.m_appliedImpulse = 0.f;
+	//	solverConstraint.m_appliedPushImpulse = 0.f;
+
 	{
 		btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
 		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
-	}
-	{
-		btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
+		solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
+	}
+	{
+		btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
 		solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
+		solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
 	}
 
@@ -483,5 +310,5 @@
 	if (body1)
 	{
-		vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
+		vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
 		denom1 = body1->getInvMass() + normalAxis.dot(vec);
 	}
@@ -492,7 +319,314 @@
 	solverConstraint.m_jacDiagABInv = denom;
 
+#ifdef _USE_JACOBIAN
+	solverConstraint.m_jac =  btJacobianEntry (
+		rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
+		body0->getInvInertiaDiagLocal(),
+		body0->getInvMass(),
+		body1->getInvInertiaDiagLocal(),
+		body1->getInvMass());
+#endif //_USE_JACOBIAN
+
+
+	{
+		btScalar rel_vel;
+		btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0)) 
+			+ solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
+		btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0)) 
+			+ solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
+
+		rel_vel = vel1Dotn+vel2Dotn;
+
+		btScalar positionalError = 0.f;
+
+		btSimdScalar velocityError =  - rel_vel;
+		btSimdScalar	velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
+		solverConstraint.m_rhs = velocityImpulse;
+		solverConstraint.m_cfm = 0.f;
+		solverConstraint.m_lowerLimit = 0;
+		solverConstraint.m_upperLimit = 1e10f;
+	}
+
 	return solverConstraint;
 }
 
+int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
+{
+	int solverBodyIdA = -1;
+
+	if (body.getCompanionId() >= 0)
+	{
+		//body has already been converted
+		solverBodyIdA = body.getCompanionId();
+	} else
+	{
+		btRigidBody* rb = btRigidBody::upcast(&body);
+		if (rb && rb->getInvMass())
+		{
+			solverBodyIdA = m_tmpSolverBodyPool.size();
+			btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+			initSolverBody(&solverBody,&body);
+			body.setCompanionId(solverBodyIdA);
+		} else
+		{
+			return 0;//assume first one is a fixed solver body
+		}
+	}
+	return solverBodyIdA;
+}
+#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())
+		{
+
+			const btVector3& pos1 = cp.getPositionWorldOnA();
+			const btVector3& pos2 = cp.getPositionWorldOnB();
+
+			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);
+				{
+#ifdef COMPUTE_IMPULSE_DENOM
+					btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
+					btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
+#else							
+					btVector3 vec;
+					btScalar denom0 = 0.f;
+					btScalar denom1 = 0.f;
+					if (rb0)
+					{
+						vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+						denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
+					}
+					if (rb1)
+					{
+						vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+						denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
+					}
+#endif //COMPUTE_IMPULSE_DENOM		
+
+					btScalar denom = relaxation/(denom0+denom1);
+					solverConstraint.m_jacDiagABInv = denom;
+				}
+
+				solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
+				solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
+				solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
+
+
+				btVector3 vel1 = rb0 ? 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;
+
+
+				solverConstraint.m_friction = cp.m_combinedFriction;
+
+				btScalar restitution = 0.f;
+				
+				if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
+				{
+					restitution = 0.f;
+				} else
+				{
+					restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
+					if (restitution <= btScalar(0.))
+					{
+						restitution = 0.f;
+					};
+				}
+
+
+				///warm starting (or zero if disabled)
+				if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+				{
+					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);
+					if (rb1)
+						m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
+				} else
+				{
+					solverConstraint.m_appliedImpulse = 0.f;
+				}
+
+				//							solverConstraint.m_appliedPushImpulse = 0.f;
+
+				{
+					btScalar rel_vel;
+					btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) 
+						+ solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
+					btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0)) 
+						+ solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
+
+					rel_vel = vel1Dotn+vel2Dotn;
+
+					btScalar positionalError = 0.f;
+					positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
+					btScalar	velocityError = restitution - rel_vel;// * damping;
+					btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+					btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+					solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+					solverConstraint.m_cfm = 0.f;
+					solverConstraint.m_lowerLimit = 0;
+					solverConstraint.m_upperLimit = 1e10f;
+				}
+
+
+				/////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);
+					}
+
+					if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
+					{
+						{
+							btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
+							if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+							{
+								frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
+								if (rb0)
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),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);
+							} else
+							{
+								frictionConstraint1.m_appliedImpulse = 0.f;
+							}
+						}
+
+						if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+						{
+							btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
+							if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+							{
+								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);
+								if (rb1)
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
+							} else
+							{
+								frictionConstraint2.m_appliedImpulse = 0.f;
+							}
+						}
+					} else
+					{
+						btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
+						frictionConstraint1.m_appliedImpulse = 0.f;
+						if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+						{
+							btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
+							frictionConstraint2.m_appliedImpulse = 0.f;
+						}
+					}
+				}
+			}
+
+
+		}
+	}
+}
 
 
@@ -506,335 +640,9 @@
 	if (!(numConstraints + numManifolds))
 	{
-//		printf("empty\n");
+		//		printf("empty\n");
 		return 0.f;
 	}
-	btPersistentManifold* manifold = 0;
-	btCollisionObject* colObj0=0,*colObj1=0;
-
-	//btRigidBody* rb0=0,*rb1=0;
-
-
-#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
-
-	BEGIN_PROFILE("refreshManifolds");
-
-	int i;
-	
-	
-
-	for (i=0;i<numManifolds;i++)
-	{
-		manifold = manifoldPtr[i];
-		rb1 = (btRigidBody*)manifold->getBody1();
-		rb0 = (btRigidBody*)manifold->getBody0();
-		
-		manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
-
-	}
-
-	END_PROFILE("refreshManifolds");
-#endif //FORCE_REFESH_CONTACT_MANIFOLDS
-
-	
-
-
-
-	//int sizeofSB = sizeof(btSolverBody);
-	//int sizeofSC = sizeof(btSolverConstraint);
-
-
-	//if (1)
-	{
-		//if m_stackAlloc, try to pack bodies/constraints to speed up solving
-//		btBlock*					sablock;
-//		sablock = stackAlloc->beginBlock();
-
-	//	int memsize = 16;
-//		unsigned char* stackMemory = stackAlloc->allocate(memsize);
-
-		
-		//todo: use stack allocator for this temp memory
-//		int minReservation = numManifolds*2;
-
-		//m_tmpSolverBodyPool.reserve(minReservation);
-
-		//don't convert all bodies, only the one we need so solver the constraints
-/*
-		{
-			for (int i=0;i<numBodies;i++)
-			{
-				btRigidBody* rb = btRigidBody::upcast(bodies[i]);
-				if (rb && 	(rb->getIslandTag() >= 0))
-				{
-					btAssert(rb->getCompanionId() < 0);
-					int solverBodyId = m_tmpSolverBodyPool.size();
-					btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-					initSolverBody(&solverBody,rb);
-					rb->setCompanionId(solverBodyId);
-				} 
-			}
-		}
-*/
-		
-		//m_tmpSolverConstraintPool.reserve(minReservation);
-		//m_tmpSolverFrictionConstraintPool.reserve(minReservation);
-
-		{
-			int i;
-
-			for (i=0;i<numManifolds;i++)
-			{
-				manifold = manifoldPtr[i];
-				colObj0 = (btCollisionObject*)manifold->getBody0();
-				colObj1 = (btCollisionObject*)manifold->getBody1();
-			
-				int solverBodyIdA=-1;
-				int solverBodyIdB=-1;
-
-				if (manifold->getNumContacts())
-				{
-
-					
-
-					if (colObj0->getIslandTag() >= 0)
-					{
-						if (colObj0->getCompanionId() >= 0)
-						{
-							//body has already been converted
-							solverBodyIdA = colObj0->getCompanionId();
-						} else
-						{
-							solverBodyIdA = m_tmpSolverBodyPool.size();
-							btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-							initSolverBody(&solverBody,colObj0);
-							colObj0->setCompanionId(solverBodyIdA);
-						}
-					} else
-					{
-						//create a static body
-						solverBodyIdA = m_tmpSolverBodyPool.size();
-						btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-						initSolverBody(&solverBody,colObj0);
-					}
-
-					if (colObj1->getIslandTag() >= 0)
-					{
-						if (colObj1->getCompanionId() >= 0)
-						{
-							solverBodyIdB = colObj1->getCompanionId();
-						} else
-						{
-							solverBodyIdB = m_tmpSolverBodyPool.size();
-							btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-							initSolverBody(&solverBody,colObj1);
-							colObj1->setCompanionId(solverBodyIdB);
-						}
-					} else
-					{
-						//create a static body
-						solverBodyIdB = m_tmpSolverBodyPool.size();
-						btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
-						initSolverBody(&solverBody,colObj1);
-					}
-				}
-
-				btVector3 rel_pos1;
-				btVector3 rel_pos2;
-				btScalar relaxation;
-
-				for (int j=0;j<manifold->getNumContacts();j++)
-				{
-					
-					btManifoldPoint& cp = manifold->getContactPoint(j);
-					
-					if (cp.getDistance() <= btScalar(0.))
-					{
-						
-						const btVector3& pos1 = cp.getPositionWorldOnA();
-						const btVector3& pos2 = cp.getPositionWorldOnB();
-
-						 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
-						 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
-
-						
-						relaxation = 1.f;
-						btScalar rel_vel;
-						btVector3 vel;
-
-						int frictionIndex = m_tmpSolverConstraintPool.size();
-
-						{
-							btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
-							btRigidBody* rb0 = btRigidBody::upcast(colObj0);
-							btRigidBody* rb1 = btRigidBody::upcast(colObj1);
-
-							solverConstraint.m_solverBodyIdA = solverBodyIdA;
-							solverConstraint.m_solverBodyIdB = solverBodyIdB;
-							solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
-
-							solverConstraint.m_originalContactPoint = &cp;
-
-							btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
-							solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
-							btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
-							solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0);
-							{
-#ifdef COMPUTE_IMPULSE_DENOM
-								btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
-								btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
-#else							
-								btVector3 vec;
-								btScalar denom0 = 0.f;
-								btScalar denom1 = 0.f;
-								if (rb0)
-								{
-									vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
-									denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
-								}
-								if (rb1)
-								{
-									vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
-									denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
-								}
-#endif //COMPUTE_IMPULSE_DENOM		
-								
-								btScalar denom = relaxation/(denom0+denom1);
-								solverConstraint.m_jacDiagABInv = denom;
-							}
-
-							solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
-							solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
-							solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
-
-
-							btVector3 vel1 = rb0 ? 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);
-							
-							solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,btScalar(0.));
-							//solverConstraint.m_penetration = cp.getDistance();
-
-							solverConstraint.m_friction = cp.m_combinedFriction;
-
-							
-							if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
-							{
-								solverConstraint.m_restitution = 0.f;
-							} else
-							{
-								solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
-								if (solverConstraint.m_restitution <= btScalar(0.))
-								{
-									solverConstraint.m_restitution = 0.f;
-								};
-							}
-
-							
-							btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
-
-							
-
-							if (solverConstraint.m_restitution > penVel)
-							{
-								solverConstraint.m_penetration = btScalar(0.);
-							}
-							 
-							
-							
-							///warm starting (or zero if disabled)
-							if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-							{
-								solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-								if (rb0)
-									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
-								if (rb1)
-									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
-							} else
-							{
-								solverConstraint.m_appliedImpulse = 0.f;
-							}
-
-							solverConstraint.m_appliedPushImpulse = 0.f;
-							
-							solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size();
-							if (!cp.m_lateralFrictionInitialized)
-							{
-								cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
-								btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
-								if (lat_rel_vel > SIMD_EPSILON)//0.0f)
-								{
-									cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
-									addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-									if(infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
-									{
-										cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
-										cp.m_lateralFrictionDir2.normalize();//??
-										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);
-									addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-									addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-									if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
-									{
-										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_FRICTION_WARMSTARTING)
-									addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-							}
-
-							if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
-							{
-								{
-									btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
-									if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-									{
-										frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
-										if (rb0)
-											m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
-										if (rb1)
-											m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
-									} else
-									{
-										frictionConstraint1.m_appliedImpulse = 0.f;
-									}
-								}
-								{
-									btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
-									if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-									{
-										frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
-										if (rb0)
-											m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
-										if (rb1)
-											m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
-									} else
-									{
-										frictionConstraint2.m_appliedImpulse = 0.f;
-									}
-								}
-							}
-						}
-
-
-					}
-				}
-			}
-		}
-	}
-	
-	btContactSolverInfo info = infoGlobal;
-
+
+	if (1)
 	{
 		int j;
@@ -845,9 +653,161 @@
 		}
 	}
-	
-	
-
-	int numConstraintPool = m_tmpSolverConstraintPool.size();
-	int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
+
+	btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+	initSolverBody(&fixedBody,0);
+
+	//btRigidBody* rb0=0,*rb1=0;
+
+	//if (1)
+	{
+		{
+
+			int totalNumRows = 0;
+			int i;
+			//calculate the total number of contraint rows
+			for (i=0;i<numConstraints;i++)
+			{
+
+				btTypedConstraint::btConstraintInfo1 info1;
+				constraints[i]->getInfo1(&info1);
+				totalNumRows += info1.m_numConstraintRows;
+			}
+			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)
+			{
+				constraints[i]->getInfo1(&info1);
+				if (info1.m_numConstraintRows)
+				{
+					btAssert(currentRow<totalNumRows);
+
+					btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
+					btTypedConstraint* constraint = constraints[i];
+
+
+
+					btRigidBody& rbA = constraint->getRigidBodyA();
+					btRigidBody& rbB = constraint->getRigidBodyB();
+
+					int solverBodyIdA = getOrInitSolverBody(rbA);
+					int solverBodyIdB = getOrInitSolverBody(rbB);
+
+					btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
+					btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
+
+					int j;
+					for ( j=0;j<info1.m_numConstraintRows;j++)
+					{
+						memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
+						currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
+						currentConstraintRow[j].m_upperLimit = FLT_MAX;
+						currentConstraintRow[j].m_appliedImpulse = 0.f;
+						currentConstraintRow[j].m_appliedPushImpulse = 0.f;
+						currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
+						currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
+					}
+
+					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);
+
+
+
+					btTypedConstraint::btConstraintInfo2 info2;
+					info2.fps = 1.f/infoGlobal.m_timeStep;
+					info2.erp = infoGlobal.m_erp;
+					info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
+					info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
+					info2.m_J2linearAxis = 0;
+					info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
+					info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
+					///the size of btSolverConstraint needs be a multiple of btScalar
+					btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
+					info2.m_constraintError = &currentConstraintRow->m_rhs;
+					info2.cfm = &currentConstraintRow->m_cfm;
+					info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
+					info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
+					constraints[i]->getInfo2(&info2);
+
+					///finalize the constraint setup
+					for ( j=0;j<info1.m_numConstraintRows;j++)
+					{
+						btSolverConstraint& solverConstraint = currentConstraintRow[j];
+
+						{
+							const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
+							solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
+						}
+						{
+							const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
+							solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
+						}
+
+						{
+							btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
+							btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
+							btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
+							btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
+
+							btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
+							sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
+							sum += iMJlB.dot(solverConstraint.m_contactNormal);
+							sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
+
+							solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
+						}
+
+
+						///fix rhs
+						///todo: add force/torque accelerators
+						{
+							btScalar rel_vel;
+							btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
+							btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
+
+							rel_vel = vel1Dotn+vel2Dotn;
+
+							btScalar restitution = 0.f;
+							btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
+							btScalar	velocityError = restitution - rel_vel;// * damping;
+							btScalar	penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+							btScalar	velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+							solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+							solverConstraint.m_appliedImpulse = 0.f;
+
+						}
+					}
+				}
+			}
+		}
+
+		{
+			int i;
+			btPersistentManifold* manifold = 0;
+			btCollisionObject* colObj0=0,*colObj1=0;
+
+
+			for (i=0;i<numManifolds;i++)
+			{
+				manifold = manifoldPtr[i];
+				convertContact(manifold,infoGlobal);
+			}
+		}
+	}
+
+	btContactSolverInfo info = infoGlobal;
+
+
+
+	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
 
 	///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
@@ -866,6 +826,4 @@
 	}
 
-
-
 	return 0.f;
 
@@ -875,6 +833,7 @@
 {
 	BT_PROFILE("solveGroupCacheFriendlyIterations");
-	int numConstraintPool = m_tmpSolverConstraintPool.size();
-	int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
+
+	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
 
 	//should traverse the contacts random order...
@@ -904,89 +863,110 @@
 			}
 
-			for (j=0;j<numConstraints;j++)
+			if (infoGlobal.m_solverMode & SOLVER_SIMD)
 			{
-				btTypedConstraint* constraint = constraints[j];
-				///todo: use solver bodies, so we don't need to copy from/to btRigidBody
-
-				if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
-				{
-					m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
-				}
-				if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
-				{
-					m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
-				}
-
-				constraint->solveConstraint(infoGlobal.m_timeStep);
-
-				if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
-				{
-					m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
-				}
-				if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
-				{
-					m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
-				}
-
+				///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);
+					}
+				}
 			}
 
-			{
-				int numPoolConstraints = m_tmpSolverConstraintPool.size();
-				for (j=0;j<numPoolConstraints;j++)
-				{
-					
-					const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
-					resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
-						m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
-				}
-			}
-
-			{
-				 int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
-				
-				 for (j=0;j<numFrictionPoolConstraints;j++)
-				{
-					const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-					btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+
-								m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;			
-
-						resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
-							m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
-							totalImpulse);
-				}
-			}
-			
-
-
-		}
+
+
+		}
+	}
+	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*/)
+{
+
 	
-		if (infoGlobal.m_splitImpulse)
-		{
-			
-			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
-			{
-				{
-					int numPoolConstraints = m_tmpSolverConstraintPool.size();
-					int j;
-					for (j=0;j<numPoolConstraints;j++)
-					{
-						const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
-
-						resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
-							m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
-					}
-				}
-			}
-
-		}
-
-	}
-
-	return 0.f;
-}
-
-
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
-{
+
+	BT_PROFILE("solveGroup");
+	//we only implement SOLVER_CACHE_FRIENDLY now
+	//you need to provide at least some bodies
+	btAssert(bodies);
+	btAssert(numBodies);
+
 	int i;
 
@@ -994,10 +974,11 @@
 	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
 
-	int numPoolConstraints = m_tmpSolverConstraintPool.size();
+	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
 	int j;
+
 	for (j=0;j<numPoolConstraints;j++)
 	{
-		
-		const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
+
+		const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
 		btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
 		btAssert(pt);
@@ -1005,10 +986,9 @@
 		if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
 		{
-			pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
-			pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
+			pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+			pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
 		}
 
 		//do a callback here?
-
 	}
 
@@ -1022,421 +1002,24 @@
 	{
 		for ( i=0;i<m_tmpSolverBodyPool.size();i++)
-        {
-                m_tmpSolverBodyPool[i].writebackVelocity();
-        }
-	}
-
-//	printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
-
-/*
-	printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
-	printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
-	printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());
-
-	
-	printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
-	printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
-	printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
-*/
+		{
+			m_tmpSolverBodyPool[i].writebackVelocity();
+		}
+	}
+
 
 	m_tmpSolverBodyPool.resize(0);
-	m_tmpSolverConstraintPool.resize(0);
-	m_tmpSolverFrictionConstraintPool.resize(0);
-
+	m_tmpSolverContactConstraintPool.resize(0);
+	m_tmpSolverNonContactConstraintPool.resize(0);
+	m_tmpSolverContactFrictionConstraintPool.resize(0);
 
 	return 0.f;
 }
 
-/// btSequentialImpulseConstraintSolver Sequentially applies impulses
-btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
-{
-	BT_PROFILE("solveGroup");
-	if (infoGlobal.m_solverMode & SOLVER_CACHE_FRIENDLY)
-	{
-		//you need to provide at least some bodies
-		//btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
-		btAssert(bodies);
-		btAssert(numBodies);
-		return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
-	}
-
-	
-
-	btContactSolverInfo info = infoGlobal;
-
-	int numiter = infoGlobal.m_numIterations;
-
-	int totalPoints = 0;
-
-
-	{
-		short j;
-		for (j=0;j<numManifolds;j++)
-		{
-			btPersistentManifold* manifold = manifoldPtr[j];
-			prepareConstraints(manifold,info,debugDrawer);
-
-			for (short p=0;p<manifoldPtr[j]->getNumContacts();p++)
-			{
-				gOrder[totalPoints].m_manifoldIndex = j;
-				gOrder[totalPoints].m_pointIndex = p;
-				totalPoints++;
-			}
-		}
-	}
-
-	{
-		int j;
-		for (j=0;j<numConstraints;j++)
-		{
-			btTypedConstraint* constraint = constraints[j];
-			constraint->buildJacobian();
-		}
-	}
-	
-
-	//should traverse the contacts random order...
-	int iteration;
-
-	{
-		for ( iteration = 0;iteration<numiter;iteration++)
-		{
-			int j;
-			if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
-			{
-				if ((iteration & 7) == 0) {
-					for (j=0; j<totalPoints; ++j) {
-						btOrderIndex tmp = gOrder[j];
-						int swapi = btRandInt2(j+1);
-						gOrder[j] = gOrder[swapi];
-						gOrder[swapi] = tmp;
-					}
-				}
-			}
-
-			for (j=0;j<numConstraints;j++)
-			{
-				btTypedConstraint* constraint = constraints[j];
-				constraint->solveConstraint(info.m_timeStep);
-			}
-
-			for (j=0;j<totalPoints;j++)
-			{
-				btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
-				solve( (btRigidBody*)manifold->getBody0(),
-									(btRigidBody*)manifold->getBody1()
-				,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
-			}
-		
-			for (j=0;j<totalPoints;j++)
-			{
-				btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
-				solveFriction((btRigidBody*)manifold->getBody0(),
-					(btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
-			}
-			
-		}
-	}
-		
-
-
-
-	return btScalar(0.);
-}
-
-
-
-
-
-
-
-void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
-{
-
-	(void)debugDrawer;
-
-	btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
-	btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
-
-
-	//only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
-	{
-#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
-		manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
-#endif //FORCE_REFESH_CONTACT_MANIFOLDS		
-		int numpoints = manifoldPtr->getNumContacts();
-
-		gTotalContactPoints += numpoints;
-
-		
-		for (int i=0;i<numpoints ;i++)
-		{
-			btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
-			if (cp.getDistance() <= btScalar(0.))
-			{
-				const btVector3& pos1 = cp.getPositionWorldOnA();
-				const btVector3& pos2 = cp.getPositionWorldOnB();
-
-				btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); 
-				btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
-				
-
-				//this jacobian entry is re-used for all iterations
-				btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
-					body1->getCenterOfMassTransform().getBasis().transpose(),
-					rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
-					body1->getInvInertiaDiagLocal(),body1->getInvMass());
-
-				
-				btScalar jacDiagAB = jac.getDiagonal();
-
-				btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
-				if (cpd)
-				{
-					//might be invalid
-					cpd->m_persistentLifeTime++;
-					if (cpd->m_persistentLifeTime != cp.getLifeTime())
-					{
-						//printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
-						new (cpd) btConstraintPersistentData;
-						cpd->m_persistentLifeTime = cp.getLifeTime();
-
-					} else
-					{
-						//printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
-						
-					}
-				} else
-				{
-						
-					//todo: should this be in a pool?
-					void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
-					cpd = new (mem)btConstraintPersistentData;
-					assert(cpd);
-
-					totalCpd ++;
-					//printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
-					cp.m_userPersistentData = cpd;
-					cpd->m_persistentLifeTime = cp.getLifeTime();
-					//printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
-					
-				}
-				assert(cpd);
-
-				cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB;
-
-				//Dependent on Rigidbody A and B types, fetch the contact/friction response func
-				//perhaps do a similar thing for friction/restutution combiner funcs...
-				
-				cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
-				cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
-				
-				btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
-				btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
-				btVector3 vel = vel1 - vel2;
-				btScalar rel_vel;
-				rel_vel = cp.m_normalWorldOnB.dot(vel);
-				
-				btScalar combinedRestitution = cp.m_combinedRestitution;
-				
-				cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
-				cpd->m_friction = cp.m_combinedFriction;
-				if (cp.m_lifeTime>info.m_restingContactRestitutionThreshold)
-				{
-					cpd->m_restitution = 0.f;
-				} else
-				{
-					cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
-					if (cpd->m_restitution <= btScalar(0.))
-					{
-						cpd->m_restitution = btScalar(0.0);
-					};
-				}
-				
-				//restitution and penetration work in same direction so
-				//rel_vel 
-				
-				btScalar penVel = -cpd->m_penetration/info.m_timeStep;
-
-				if (cpd->m_restitution > penVel)
-				{
-					cpd->m_penetration = btScalar(0.);
-				} 				
-				
-
-				btScalar relaxation = info.m_damping;
-				if (info.m_solverMode & SOLVER_USE_WARMSTARTING)
-				{
-					cpd->m_appliedImpulse *= relaxation;
-				} else
-				{
-					cpd->m_appliedImpulse =btScalar(0.);
-				}
-	
-				//for friction
-				cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
-				
-				//re-calculate friction direction every frame, todo: check if this is really needed
-				btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
-
-
-#define NO_FRICTION_WARMSTART 1
-
-	#ifdef NO_FRICTION_WARMSTART
-				cpd->m_accumulatedTangentImpulse0 = btScalar(0.);
-				cpd->m_accumulatedTangentImpulse1 = btScalar(0.);
-	#endif //NO_FRICTION_WARMSTART
-				btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
-				btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
-				btScalar denom = relaxation/(denom0+denom1);
-				cpd->m_jacDiagABInvTangent0 = denom;
-
-
-				denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
-				denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
-				denom = relaxation/(denom0+denom1);
-				cpd->m_jacDiagABInvTangent1 = denom;
-
-				btVector3 totalImpulse = 
-	#ifndef NO_FRICTION_WARMSTART
-					cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
-					cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
-	#endif //NO_FRICTION_WARMSTART
-					cp.m_normalWorldOnB*cpd->m_appliedImpulse;
-
-
-
-				///
-				{
-				btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
-				cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
-				btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
-				cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
-				}
-				{
-					btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
-					cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
-				}
-				{
-					btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
-					cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
-				}
-				{
-					btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
-					cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
-				}
-				{
-					btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
-					cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
-				}
-				
-				///
-
-
-
-				//apply previous frames impulse on both bodies
-				body0->applyImpulse(totalImpulse, rel_pos1);
-				body1->applyImpulse(-totalImpulse, rel_pos2);
-			}
-			
-		}
-	}
-}
-
-
-btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
-{
-	btScalar maxImpulse = btScalar(0.);
-
-	{
-
-		
-		{
-			if (cp.getDistance() <= btScalar(0.))
-			{
-
-				
-
-				{
-
-					//btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
-					btScalar impulse = resolveSingleCollisionCombined(
-						*body0,*body1,
-						cp,
-						info);
-
-					if (maxImpulse < impulse)
-						maxImpulse  = impulse;
-
-				}
-			}
-		}
-	}
-	return maxImpulse;
-}
-
-
-
-btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
-{
-
-	btScalar maxImpulse = btScalar(0.);
-
-	{
-
-		
-		{
-			if (cp.getDistance() <= btScalar(0.))
-			{
-
-				
-
-				{
-
-					btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
-					btScalar impulse = cpd->m_contactSolverFunc(
-						*body0,*body1,
-						cp,
-						info);
-
-					if (maxImpulse < impulse)
-						maxImpulse  = impulse;
-
-				}
-			}
-		}
-	}
-	return maxImpulse;
-}
-
-btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
-{
-
-	(void)debugDrawer;
-	(void)iter;
-
-
-	{
-
-		
-		{
-			
-			if (cp.getDistance() <= btScalar(0.))
-			{
-
-				btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
-				cpd->m_frictionSolverFunc(
-					*body0,*body1,
-					cp,
-					info);
-
-				
-			}
-		}
-
-	
-	}
-	return btScalar(0.);
-}
+
+
+
+
+
+
 
 
Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h	(revision 2907)
@@ -24,65 +24,57 @@
 
 
-///The btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
-///The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com
-///Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP)
-///Applies impulses for combined restitution and penetration recovery and to simulate friction
+
+///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
 class btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
+protected:
 
 	btAlignedObjectArray<btSolverBody>	m_tmpSolverBodyPool;
-	btAlignedObjectArray<btSolverConstraint>	m_tmpSolverConstraintPool;
-	btAlignedObjectArray<btSolverConstraint>	m_tmpSolverFrictionConstraintPool;
+	btConstraintArray			m_tmpSolverContactConstraintPool;
+	btConstraintArray			m_tmpSolverNonContactConstraintPool;
+	btConstraintArray			m_tmpSolverContactFrictionConstraintPool;
 	btAlignedObjectArray<int>	m_orderTmpConstraintPool;
 	btAlignedObjectArray<int>	m_orderFrictionConstraintPool;
 
-
-protected:
-	btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
-	btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
-	void  prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
 	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);
-
-	ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
-	ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
-
 	
 	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
 	unsigned long	m_btSeed2;
 
+	void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
+	btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
+
+	void	convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
+
+	void	resolveSplitPenetrationImpulseCacheFriendly(
+        btSolverBody& body1,
+        btSolverBody& body2,
+        const btSolverConstraint& contactConstraint,
+        const btContactSolverInfo& solverInfo);
+
+	//internal method
+	int	getOrInitSolverBody(btCollisionObject& body);
+
+	void	resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+
+	void	resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+	
+	void	resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+	
+	void	resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+		
 public:
 
 	
 	btSequentialImpulseConstraintSolver();
+	virtual ~btSequentialImpulseConstraintSolver();
 
-	///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
-	///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
-	void	setContactSolverFunc(ContactSolverFunc func,int type0,int type1)
-	{
-		m_contactDispatch[type0][type1] = func;
-	}
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
 	
-	///Advanced: Override the default friction solving function for contacts, for certain types of rigidbody
-	///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
-	void	SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1)
-	{
-		m_frictionDispatch[type0][type1] = func;
-	}
-
-	virtual ~btSequentialImpulseConstraintSolver();
-	
-	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
-
-	virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+	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);
-	btScalar solveGroupCacheFriendlySetup(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();
-
-	btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
-
-
 	
 	unsigned long btRand2();
Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp	(revision 2907)
@@ -69,5 +69,7 @@
 btSliderConstraint::btSliderConstraint()
         :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
-		m_useLinearReferenceFrameA(true)
+		m_useLinearReferenceFrameA(true),
+		m_useSolveConstraintObsolete(false)
+//		m_useSolveConstraintObsolete(true)
 {
 	initParams();
@@ -80,5 +82,7 @@
         , m_frameInA(frameInA)
         , m_frameInB(frameInB),
-		m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+		m_useLinearReferenceFrameA(useLinearReferenceFrameA),
+		m_useSolveConstraintObsolete(false)
+//		m_useSolveConstraintObsolete(true)
 {
 	initParams();
@@ -89,4 +93,8 @@
 void btSliderConstraint::buildJacobian()
 {
+	if (!m_useSolveConstraintObsolete) 
+	{
+		return;
+	}
 	if(m_useLinearReferenceFrameA)
 	{
@@ -156,14 +164,357 @@
 //-----------------------------------------------------------------------------
 
-void btSliderConstraint::solveConstraint(btScalar timeStep)
-{
-    m_timeStep = timeStep;
-	if(m_useLinearReferenceFrameA)
-	{
-		solveConstraintInt(m_rbA, m_rbB);
+void btSliderConstraint::getInfo1(btConstraintInfo1* info)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
 	}
 	else
 	{
-		solveConstraintInt(m_rbB, m_rbA);
+		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)
+{
+	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
+	// the slider axis should be the only unconstrained
+	// rotational axis, the angular velocity of the two bodies perpendicular to
+	// the slider 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 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];
+	info->m_J1angularAxis[2] = p[2];
+	info->m_J1angularAxis[s+0] = q[0];
+	info->m_J1angularAxis[s+1] = q[1];
+	info->m_J1angularAxis[s+2] = q[2];
+
+	info->m_J2angularAxis[0] = -p[0];
+	info->m_J2angularAxis[1] = -p[1];
+	info->m_J2angularAxis[2] = -p[2];
+	info->m_J2angularAxis[s+0] = -q[0];
+	info->m_J2angularAxis[s+1] = -q[1];
+	info->m_J2angularAxis[s+2] = -q[2];
+	// 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 "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.
+	btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
+    btVector3 ax2 = trB.getBasis().getColumn(0);
+	btVector3 u = ax1.cross(ax2);
+	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
+	int srow;
+	// check linear limits linear
+	btScalar limit_err = btScalar(0.0);
+	int limit = 0;
+	if(getSolveLinLimit())
+	{
+		limit_err = getLinDepth() *  signFact;
+		limit = (limit_err > btScalar(0.0)) ? 2 : 1;
+	}
+	int powered = 0;
+	if(getPoweredLinMotor())
+	{
+		powered = 1;
+	}
+	// if the slider has joint limits or motor, add in the extra row
+	if (limit || powered) 
+	{
+		nrow++;
+		srow = nrow * info->rowskip;
+		info->m_J1linearAxis[srow+0] = ax1[0];
+		info->m_J1linearAxis[srow+1] = ax1[1];
+		info->m_J1linearAxis[srow+2] = ax1[2];
+		// linear torque decoupling step:
+		//
+		// we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
+		// do not create a torque couple. in other words, the points that the
+		// constraint force is applied at must lie along the same ax1 axis.
+		// 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];
+		// right-hand part
+		btScalar lostop = getLowerLinLimit();
+		btScalar histop = getUpperLinLimit();
+		if(limit && (lostop == histop))
+		{  // the joint motor is ineffective
+			powered = 0;
+		}
+		info->m_constraintError[srow] = 0.;
+		info->m_lowerLimit[srow] = 0.;
+		info->m_upperLimit[srow] = 0.;
+		if(powered)
+		{
+            info->cfm[nrow] = btScalar(0.0); 
+			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();
+			info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
+			info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
+			info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
+		}
+		if(limit)
+		{
+			k = info->fps * info->erp;
+			info->m_constraintError[srow] += k * limit_err;
+			info->cfm[srow] = btScalar(0.0); // stop_cfm;
+			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] = -SIMD_INFINITY;
+				info->m_upperLimit[srow] = 0;
+			}
+			else 
+			{ // high limit
+				info->m_lowerLimit[srow] = 0;
+				info->m_upperLimit[srow] = SIMD_INFINITY;
+			}
+			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
+			btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
+			if(bounce > btScalar(0.0))
+			{
+				btScalar vel = m_rbA.getLinearVelocity().dot(ax1);
+				vel -= m_rbB.getLinearVelocity().dot(ax1);
+				vel *= signFact;
+				// 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] *= getSoftnessLimLin();
+		} // if(limit)
+	} // if linear limit
+	// check angular limits
+	limit_err = btScalar(0.0);
+	limit = 0;
+	if(getSolveAngLimit())
+	{
+		limit_err = getAngDepth();
+		limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+	}
+	// if the slider has joint limits, add in the extra row
+	powered = 0;
+	if(getPoweredAngMotor())
+	{
+		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 = getLowerAngLimit();
+		btScalar histop = getUpperAngLimit();
+		if(limit && (lostop == histop))
+		{  // the joint motor is ineffective
+			powered = 0;
+		}
+		if(powered)
+		{
+            info->cfm[srow] = btScalar(0.0); 
+			btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
+			info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
+			info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
+			info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
+		}
+		if(limit)
+		{
+			k = info->fps * info->erp;
+			info->m_constraintError[srow] += k * limit_err;
+			info->cfm[srow] = btScalar(0.0); // stop_cfm;
+			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 = btFabs(btScalar(1.0) - getDampingLimAng());
+			if(bounce > btScalar(0.0))
+			{
+				btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
+				vel -= m_rbB.getAngularVelocity().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] *= getSoftnessLimAng();
+		} // 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);
+		}
+		else
+		{
+			solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
+		}
 	}
 } // btSliderConstraint::solveConstraint()
@@ -171,10 +522,12 @@
 //-----------------------------------------------------------------------------
 
-void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
+void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
 {
     int i;
     // linear
-    btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA);
-    btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB);
+    btVector3 velA;
+	bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
+    btVector3 velB;
+	bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
     btVector3 vel = velA - velB;
 	for(i = 0; i < 3; i++)
@@ -191,6 +544,16 @@
 		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);
+		
+		//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
@@ -218,6 +581,16 @@
 				// apply clamped impulse
 				impulse_vector = normal * normalImpulse;
-				rbA.applyImpulse( impulse_vector, m_relPosA);
-				rbB.applyImpulse(-impulse_vector, m_relPosB);
+				//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);
+				}
+
+
+
 			}
 		}
@@ -228,6 +601,8 @@
 	btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);
 
-	const btVector3& angVelA = rbA.getAngularVelocity();
-	const btVector3& angVelB = rbB.getAngularVelocity();
+	btVector3 angVelA;
+	bodyA.getAngularVelocity(angVelA);
+	btVector3 angVelB;
+	bodyB.getAngularVelocity(angVelB);
 
 	btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
@@ -239,12 +614,18 @@
 	//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;
+		//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))
@@ -252,9 +633,17 @@
 		btVector3 normal2 = angularError.normalized();
 		btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
-		angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
+		angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
+		angularError *= angularImpulseMag;
 	}
 	// apply impulse
-	rbA.applyTorqueImpulse(-velrelOrthog+angularError);
-	rbB.applyTorqueImpulse(velrelOrthog-angularError);
+	//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
@@ -270,6 +659,12 @@
 	}
 	btVector3 impulse = axisA * impulseMag;
-	rbA.applyTorqueImpulse(impulse);
-	rbB.applyTorqueImpulse(-impulse);
+	//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) 
@@ -302,6 +697,9 @@
 			// apply clamped impulse
 			btVector3 motorImp = angImpulse * axisA;
-			m_rbA.applyTorqueImpulse(motorImp);
-			m_rbB.applyTorqueImpulse(-motorImp);
+			//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);
 		}
 	}
@@ -313,5 +711,5 @@
 
 void btSliderConstraint::calculateTransforms(void){
-	if(m_useLinearReferenceFrameA)
+	if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
 	{
 		m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
@@ -326,5 +724,12 @@
 	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;
     btVector3 normalWorld;
@@ -368,5 +773,4 @@
 
 //-----------------------------------------------------------------------------
- 
 
 void btSliderConstraint::testAngLimits(void)
@@ -380,4 +784,5 @@
 		const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
 		btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));  
+		m_angPos = rot;
 		if(rot < m_lowerAngLimit)
 		{
@@ -392,9 +797,6 @@
 	}
 } // btSliderConstraint::testAngLimits()
-
 	
 //-----------------------------------------------------------------------------
-
-
 
 btVector3 btSliderConstraint::getAncorInA(void)
Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h	(revision 2907)
@@ -47,4 +47,6 @@
 {
 protected:
+	///for backwards compatibility during the transition to 'getInfo/getInfo2'
+	bool		m_useSolveConstraintObsolete;
 	btTransform	m_frameInA;
     btTransform	m_frameInB;
@@ -105,4 +107,5 @@
 
 	btScalar m_linPos;
+	btScalar m_angPos;
 
 	btScalar m_angDepth;
@@ -127,5 +130,11 @@
 	// overrides
     virtual void	buildJacobian();
-    virtual	void	solveConstraint(btScalar	timeStep);
+    virtual void getInfo1 (btConstraintInfo1* info);
+	
+	virtual void getInfo2 (btConstraintInfo2* info);
+
+    virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep);
+	
+
 	// access
     const btRigidBody& getRigidBodyA() const { return m_rbA; }
@@ -195,4 +204,5 @@
 	btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
 	btScalar getLinearPos() { return m_linPos; }
+	
 
 	// access for ODE solver
@@ -203,8 +213,9 @@
 	// internal
     void	buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
-    void	solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB);
+    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);
 	// access for PE Solver
Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h	(revision 2907)
@@ -24,4 +24,84 @@
 #include "LinearMath/btTransformUtil.h"
 
+///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
+#ifdef BT_USE_SSE
+#define USE_SIMD 1
+#endif //
+
+
+#ifdef USE_SIMD
+
+struct	btSimdScalar
+{
+	SIMD_FORCE_INLINE	btSimdScalar()
+	{
+
+	}
+
+	SIMD_FORCE_INLINE	btSimdScalar(float	fl)
+	:m_vec128 (_mm_set1_ps(fl))
+	{
+	}
+
+	SIMD_FORCE_INLINE	btSimdScalar(__m128 v128)
+		:m_vec128(v128)
+	{
+	}
+	union
+	{
+		__m128		m_vec128;
+		float		m_floats[4];
+		int			m_ints[4];
+		btScalar	m_unusedPadding;
+	};
+	SIMD_FORCE_INLINE	__m128	get128()
+	{
+		return m_vec128;
+	}
+
+	SIMD_FORCE_INLINE	const __m128	get128() const
+	{
+		return m_vec128;
+	}
+
+	SIMD_FORCE_INLINE	void	set128(__m128 v128)
+	{
+		m_vec128 = v128;
+	}
+
+	SIMD_FORCE_INLINE	operator       __m128()       
+	{ 
+		return m_vec128; 
+	}
+	SIMD_FORCE_INLINE	operator const __m128() const 
+	{ 
+		return m_vec128; 
+	}
+	
+	SIMD_FORCE_INLINE	operator float() const 
+	{ 
+		return m_floats[0]; 
+	}
+
+};
+
+///@brief Return the elementwise product of two btSimdScalar
+SIMD_FORCE_INLINE btSimdScalar 
+operator*(const btSimdScalar& v1, const btSimdScalar& v2) 
+{
+	return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
+}
+
+///@brief Return the elementwise product of two btSimdScalar
+SIMD_FORCE_INLINE btSimdScalar 
+operator+(const btSimdScalar& v1, const btSimdScalar& v2) 
+{
+	return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
+}
+
+
+#else
+#define btSimdScalar btScalar
+#endif
 
 ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
@@ -29,43 +109,43 @@
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
+	btVector3		m_deltaLinearVelocity;
+	btVector3		m_deltaAngularVelocity;
+	btScalar		m_angularFactor;
+	btScalar		m_invMass;
+	btScalar		m_friction;
+	btRigidBody*	m_originalBody;
+	btVector3		m_pushVelocity;
+	//btVector3		m_turnVelocity;	
+
 	
-	btMatrix3x3		m_worldInvInertiaTensor;
-
-	btVector3		m_angularVelocity;
-	btVector3		m_linearVelocity;
-	btVector3		m_centerOfMassPosition;
-	btVector3		m_pushVelocity;
-	btVector3		m_turnVelocity;
-
-	float			m_angularFactor;
-	float			m_invMass;
-	float			m_friction;
-	btRigidBody*	m_originalBody;
-	
-	
-	SIMD_FORCE_INLINE void	getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
+	SIMD_FORCE_INLINE void	getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
 	{
-		velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
+		if (m_originalBody)
+			velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
+		else
+			velocity.setValue(0,0,0);
 	}
 
+	SIMD_FORCE_INLINE void	getAngularVelocity(btVector3& angVel) const
+	{
+		if (m_originalBody)
+			angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
+		else
+			angVel.setValue(0,0,0);
+	}
+
+
 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
 	{
-		if (m_invMass)
+		//if (m_invMass)
 		{
-			m_linearVelocity += linearComponent*impulseMagnitude;
-			m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
-		}
-	}
-	
-	SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
-	{
-		if (m_invMass)
-		{
-			m_pushVelocity += linearComponent*impulseMagnitude;
-			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+			m_deltaLinearVelocity += linearComponent*impulseMagnitude;
+			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
 		}
 	}
 
+	
+/*
 	
 	void	writebackVelocity()
@@ -73,37 +153,21 @@
 		if (m_invMass)
 		{
-			m_originalBody->setLinearVelocity(m_linearVelocity);
-			m_originalBody->setAngularVelocity(m_angularVelocity);
+			m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
+			m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
 			
 			//m_originalBody->setCompanionId(-1);
 		}
 	}
-	
+	*/
 
-	void	writebackVelocity(btScalar timeStep)
+	void	writebackVelocity(btScalar timeStep=0)
 	{
 		if (m_invMass)
 		{
-			m_originalBody->setLinearVelocity(m_linearVelocity);
-			m_originalBody->setAngularVelocity(m_angularVelocity);
-			
-			//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->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
+			m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
 			//m_originalBody->setCompanionId(-1);
 		}
 	}
-
-	void	readVelocity()
-	{
-		if (m_invMass)
-		{
-			m_linearVelocity = m_originalBody->getLinearVelocity();
-			m_angularVelocity = m_originalBody->getAngularVelocity();
-		}
-	}
-
 	
 
Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h	(revision 2907)
@@ -20,6 +20,9 @@
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btMatrix3x3.h"
+#include "btJacobianEntry.h"
 
 //#define NO_FRICTION_TANGENTIALS 1
+#include "btSolverBody.h"
+
 
 ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
@@ -28,30 +31,51 @@
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btVector3	m_relpos1CrossNormal;
-	btVector3	m_contactNormal;
+	btVector3		m_relpos1CrossNormal;
+	btVector3		m_contactNormal;
 
-	btVector3	m_relpos2CrossNormal;
-	btVector3	m_angularComponentA;
+	btVector3		m_relpos2CrossNormal;
+	//btVector3		m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
 
-	btVector3	m_angularComponentB;
-
-	mutable btScalar	m_appliedPushImpulse;
+	btVector3		m_angularComponentA;
+	btVector3		m_angularComponentB;
 	
-	mutable btScalar	m_appliedImpulse;
-	int			m_solverBodyIdA;
-	int			m_solverBodyIdB;
+	mutable btSimdScalar	m_appliedPushImpulse;
+	mutable btSimdScalar	m_appliedImpulse;
+	
 	
 	btScalar	m_friction;
-	btScalar	m_restitution;
 	btScalar	m_jacDiagABInv;
-	btScalar	m_penetration;
+	union
+	{
+		int	m_numConsecutiveRowsPerKernel;
+		btScalar	m_unusedPadding0;
+	};
+
+	union
+	{
+		int			m_frictionIndex;
+		btScalar	m_unusedPadding1;
+	};
+	union
+	{
+		int			m_solverBodyIdA;
+		btScalar	m_unusedPadding2;
+	};
+	union
+	{
+		int			m_solverBodyIdB;
+		btScalar	m_unusedPadding3;
+	};
 	
+	union
+	{
+		void*		m_originalContactPoint;
+		btScalar	m_unusedPadding4;
+	};
 
-	
-	int			m_constraintType;
-	int			m_frictionIndex;
-	void*		m_originalContactPoint;
-	int			m_unusedPadding[1];
-
+	btScalar		m_rhs;
+	btScalar		m_cfm;
+	btScalar		m_lowerLimit;
+	btScalar		m_upperLimit;
 
 	enum		btSolverConstraintType
@@ -62,7 +86,5 @@
 };
 
-
-
-
+typedef btAlignedObjectArray<btSolverConstraint>	btConstraintArray;
 
 
Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp	(revision 2907)
@@ -20,4 +20,6 @@
 static btRigidBody s_fixed(0, 0,0);
 
+#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
+
 btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
 :m_userConstraintType(-1),
@@ -26,5 +28,6 @@
 m_rbA(s_fixed),
 m_rbB(s_fixed),
-m_appliedImpulse(btScalar(0.))
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
 {
 	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
@@ -36,5 +39,6 @@
 m_rbA(rbA),
 m_rbB(s_fixed),
-m_appliedImpulse(btScalar(0.))
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
 {
 		s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
@@ -49,5 +53,6 @@
 m_rbA(rbA),
 m_rbB(rbB),
-m_appliedImpulse(btScalar(0.))
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
 {
 		s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
@@ -55,2 +60,55 @@
 }
 
+
+//-----------------------------------------------------------------------------
+
+btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
+{
+	if(lowLim > uppLim)
+	{
+		return btScalar(1.0f);
+	}
+	else if(lowLim == uppLim)
+	{
+		return btScalar(0.0f);
+	}
+	btScalar lim_fact = btScalar(1.0f);
+	btScalar delta_max = vel / timeFact;
+	if(delta_max < btScalar(0.0f))
+	{
+		if((pos >= lowLim) && (pos < (lowLim - delta_max)))
+		{
+			lim_fact = (lowLim - pos) / delta_max;
+		}
+		else if(pos  < lowLim)
+		{
+			lim_fact = btScalar(0.0f);
+		}
+		else
+		{
+			lim_fact = btScalar(1.0f);
+		}
+	}
+	else if(delta_max > btScalar(0.0f))
+	{
+		if((pos <= uppLim) && (pos > (uppLim - delta_max)))
+		{
+			lim_fact = (uppLim - pos) / delta_max;
+		}
+		else if(pos  > uppLim)
+		{
+			lim_fact = btScalar(0.0f);
+		}
+		else
+		{
+			lim_fact = btScalar(1.0f);
+		}
+	}
+	else
+	{
+			lim_fact = btScalar(0.0f);
+	}
+	return lim_fact;
+} // btTypedConstraint::getMotorFactor()
+
+
Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h	(revision 2907)
@@ -19,4 +19,9 @@
 class btRigidBody;
 #include "LinearMath/btScalar.h"
+#include "btSolverConstraint.h"
+struct  btSolverBody;
+
+
+
 
 enum btTypedConstraintType
@@ -26,5 +31,4 @@
 	CONETWIST_CONSTRAINT_TYPE,
 	D6_CONSTRAINT_TYPE,
-	VEHICLE_CONSTRAINT_TYPE,
 	SLIDER_CONSTRAINT_TYPE
 };
@@ -49,4 +53,5 @@
 	btRigidBody&	m_rbB;
 	btScalar	m_appliedImpulse;
+	btScalar	m_dbgDrawSize;
 
 
@@ -56,11 +61,53 @@
 	virtual ~btTypedConstraint() {};
 	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
+	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
 
-	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
+	struct btConstraintInfo1 {
+		int m_numConstraintRows,nub;
+	};
+
+	struct btConstraintInfo2 {
+		// integrator parameters: frames per second (1/stepsize), default error
+		// reduction parameter (0..1).
+		btScalar fps,erp;
+
+		// for the first and second body, pointers to two (linear and angular)
+		// n*3 jacobian sub matrices, stored by rows. these matrices will have
+		// been initialized to 0 on entry. if the second body is zero then the
+		// J2xx pointers may be 0.
+		btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
+
+		// elements to jump from one row to the next in J's
+		int rowskip;
+
+		// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
+		// "constraint force mixing" vector. c is set to zero on entry, cfm is
+		// set to a constant value (typically very small or zero) value on entry.
+		btScalar *m_constraintError,*cfm;
+
+		// lo and hi limits for variables (set to -/+ infinity on entry).
+		btScalar *m_lowerLimit,*m_upperLimit;
+
+		// findex vector for variables. see the LCP solver interface for a
+		// description of what this does. this is set to -1 on entry.
+		// note that the returned indexes are relative to the first index of
+		// the constraint.
+		int *findex;
+	};
+
 
 	virtual void	buildJacobian() = 0;
 
-	virtual	void	solveConstraint(btScalar	timeStep) = 0;
+	virtual	void	setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
+	{
+	}
+	virtual void getInfo1 (btConstraintInfo1* info)=0;
 
+	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);
+	
 	const btRigidBody& getRigidBodyA() const
 	{
@@ -95,5 +142,5 @@
 		m_userConstraintId = uid;
 	}
-	
+
 	int getUserConstraintId() const
 	{
@@ -115,5 +162,14 @@
 		return m_constraintType;
 	}
-
+	
+	void setDbgDrawSize(btScalar dbgDrawSize)
+	{
+		m_dbgDrawSize = dbgDrawSize;
+	}
+	btScalar getDbgDrawSize()
+	{
+		return m_dbgDrawSize;
+	}
+	
 };
 
