diff --git a/Demos/MultiThreadedDemo/MultiThreadedDemo.cpp b/Demos/MultiThreadedDemo/MultiThreadedDemo.cpp
index 6c633e416..62b2cd10a 100644
--- a/Demos/MultiThreadedDemo/MultiThreadedDemo.cpp
+++ b/Demos/MultiThreadedDemo/MultiThreadedDemo.cpp
@@ -102,7 +102,7 @@ void MultiThreadedDemo::createStack( btCollisionShape* boxShape, float halfCubeS
 		int rowSize = size - i;
 		for(int j=0; j< rowSize; j++)
 		{
-			btVector4 pos;
+			btVector3 pos;
 			pos.setValue(
 				-rowSize * halfCubeSize + halfCubeSize + j * 2.0f * halfCubeSize,
 				halfCubeSize + i * halfCubeSize * 2.0f,
@@ -148,9 +148,10 @@ void MultiThreadedDemo::clientMoveAndDisplay()
 	if (m_dynamicsWorld)
 	{
 
-//#define FIXED_STEP 1
+#define FIXED_STEP 1
 #ifdef FIXED_STEP
   		m_dynamicsWorld->stepSimulation(1.0f/60.f,0);
+		//CProfileManager::dumpAll();
 	
 #else
 		//during idle mode, just run 1 simulation step maximum
@@ -354,6 +355,7 @@ m_threadSupportCollision = new Win32ThreadSupport(Win32ThreadSupport::Win32Threa
 		m_dynamicsWorld = world;
 
 		world->getSolverInfo().m_numIterations = 4;
+		world->getSolverInfo().m_solverMode = SOLVER_SIMD+SOLVER_USE_WARMSTARTING;
 
 		m_dynamicsWorld->getDispatchInfo().m_enableSPU = true;
 		m_dynamicsWorld->setGravity(btVector3(0,-10,0));
diff --git a/Demos/OpenGL/DemoApplication.h b/Demos/OpenGL/DemoApplication.h
index 881daa680..c43711c8b 100644
--- a/Demos/OpenGL/DemoApplication.h
+++ b/Demos/OpenGL/DemoApplication.h
@@ -102,6 +102,7 @@ public:
 
 	}
 
+	
 	void setOrthographicProjection();
 	void resetPerspectiveProjection();
 	
diff --git a/Extras/quickstep/btOdeQuickstepConstraintSolver.cpp b/Extras/quickstep/btOdeQuickstepConstraintSolver.cpp
index b0489195c..9c461f5af 100644
--- a/Extras/quickstep/btOdeQuickstepConstraintSolver.cpp
+++ b/Extras/quickstep/btOdeQuickstepConstraintSolver.cpp
@@ -206,8 +206,8 @@ int btOdeQuickstepConstraintSolver::ConvertBody(btRigidBody* orgBody,btAlignedOb
 
     body->m_originalBody = orgBody;
 
-    body->m_facc.setValue(0,0,0,0);
-    body->m_tacc.setValue(0,0,0,0);
+    body->m_facc.setValue(0,0,0);
+    body->m_tacc.setValue(0,0,0);
 
     body->m_linearVelocity = orgBody->getLinearVelocity();
     body->m_angularVelocity = orgBody->getAngularVelocity();
diff --git a/Extras/quickstep/btOdeTypedJoint.cpp b/Extras/quickstep/btOdeTypedJoint.cpp
index c289bed6f..74dc003eb 100644
--- a/Extras/quickstep/btOdeTypedJoint.cpp
+++ b/Extras/quickstep/btOdeTypedJoint.cpp
@@ -96,16 +96,6 @@ void OdeP2PJoint::GetInfo2(Info2 *info)
     {
         body0_trans = m_body0->m_originalBody->getCenterOfMassTransform();
     }
-//    btScalar body0_mat[12];
-//    body0_mat[0] = body0_trans.getBasis()[0][0];
-//    body0_mat[1] = body0_trans.getBasis()[0][1];
-//    body0_mat[2] = body0_trans.getBasis()[0][2];
-//    body0_mat[4] = body0_trans.getBasis()[1][0];
-//    body0_mat[5] = body0_trans.getBasis()[1][1];
-//    body0_mat[6] = body0_trans.getBasis()[1][2];
-//    body0_mat[8] = body0_trans.getBasis()[2][0];
-//    body0_mat[9] = body0_trans.getBasis()[2][1];
-//    body0_mat[10] = body0_trans.getBasis()[2][2];
 
     btTransform body1_trans;
 
@@ -113,23 +103,8 @@ void OdeP2PJoint::GetInfo2(Info2 *info)
     {
         body1_trans = m_body1->m_originalBody->getCenterOfMassTransform();
     }
-//    btScalar body1_mat[12];
-//    body1_mat[0] = body1_trans.getBasis()[0][0];
-//    body1_mat[1] = body1_trans.getBasis()[0][1];
-//    body1_mat[2] = body1_trans.getBasis()[0][2];
-//    body1_mat[4] = body1_trans.getBasis()[1][0];
-//    body1_mat[5] = body1_trans.getBasis()[1][1];
-//    body1_mat[6] = body1_trans.getBasis()[1][2];
-//    body1_mat[8] = body1_trans.getBasis()[2][0];
-//    body1_mat[9] = body1_trans.getBasis()[2][1];
-//    body1_mat[10] = body1_trans.getBasis()[2][2];
-
-
-
 
     // anchor points in global coordinates with respect to body PORs.
-
-
     int s = info->rowskip;
 
     // set jacobian
@@ -160,17 +135,18 @@ void OdeP2PJoint::GetInfo2(Info2 *info)
     {
         for (int j=0; j<3; j++)
         {
-            info->m_constraintError[j] = k * (a2[j] + body1_trans.getOrigin()[j] -
-                              a1[j] - body0_trans.getOrigin()[j]);
+            info->m_constraintError[j] = 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]);
         }
     }
     else
     {
         for (int j=0; j<3; j++)
         {
-            info->m_constraintError[j] = k * (p2pconstraint->getPivotInB()[j] - a1[j] -
-                              body0_trans.getOrigin()[j]);
-        }
+		    info->m_constraintError[j] = k * (p2pconstraint->getPivotInB()[j] - a1[j] -                       body0_trans.getOrigin()[j]);
+        	printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
+        
+		}
     }
 }
 
diff --git a/src/BulletCollision/CollisionShapes/btBoxShape.h b/src/BulletCollision/CollisionShapes/btBoxShape.h
index eaece37dc..30bc64556 100644
--- a/src/BulletCollision/CollisionShapes/btBoxShape.h
+++ b/src/BulletCollision/CollisionShapes/btBoxShape.h
@@ -161,28 +161,22 @@ public:
 		switch (i)
 		{
 		case 0:
-			plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.));
-			plane[3] = -halfExtents.x();
+			plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x());
 			break;
 		case 1:
-			plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.));
-			plane[3] = -halfExtents.x();
+			plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x());
 			break;
 		case 2:
-			plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.));
-			plane[3] = -halfExtents.y();
+			plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y());
 			break;
 		case 3:
-			plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.));
-			plane[3] = -halfExtents.y();
+			plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y());
 			break;
 		case 4:
-			plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.));
-			plane[3] = -halfExtents.z();
+			plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z());
 			break;
 		case 5:
-			plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.));
-			plane[3] = -halfExtents.z();
+			plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z());
 			break;
 		default:
 			assert(0);
diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
index c33d92459..e760347fb 100644
--- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
@@ -23,7 +23,8 @@ Written by: Marcus Hennix
 #include <new>
 
 btConeTwistConstraint::btConeTwistConstraint()
-:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE)
+:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
+m_useSolveConstraintObsolete(true)
 {
 }
 
@@ -31,7 +32,8 @@ btConeTwistConstraint::btConeTwistConstraint()
 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, 
 											 const btTransform& rbAFrame,const btTransform& rbBFrame)
 											 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
-											 m_angularOnly(false)
+											 m_angularOnly(false),
+											 m_useSolveConstraintObsolete(true)
 {
 	m_swingSpan1 = btScalar(1e30);
 	m_swingSpan2 = btScalar(1e30);
@@ -46,7 +48,8 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,
 
 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
 											:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
-											 m_angularOnly(false)
+											 m_angularOnly(false),
+											 m_useSolveConstraintObsolete(true)
 {
 	m_rbBFrame = m_rbAFrame;
 	
@@ -61,221 +64,251 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform&
 	
 }			
 
+
+void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
+	} else
+	{
+		btAssert(0);
+	}
+}
+	
+void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	btAssert(0);
+}
+	
+
 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)
+	if (m_useSolveConstraintObsolete)
 	{
-		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
-		btVector3 relPos = pivotBInW - pivotAInW;
+		m_appliedImpulse = btScalar(0.);
 
-		btVector3 normal[3];
-		if (relPos.length2() > SIMD_EPSILON)
+		//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)
 		{
-			normal[0] = relPos.normalized();
-		}
-		else
-		{
-			normal[0].setValue(btScalar(1.0),0,0);
-		}
+			btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+			btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+			btVector3 relPos = pivotBInW - pivotAInW;
 
-		btPlaneSpace1(normal[0], normal[1], normal[2]);
-
-		for (int i=0;i<3;i++)
-		{
-			new (&m_jac[i]) btJacobianEntry(
-				pivotAInW - m_rbA.getCenterOfMassPosition(),
-				pivotBInW - m_rbB.getCenterOfMassPosition(),
-				normal[i],
-				m_rbA.getInvInertiaDiagLocal(),
-				m_rbA.getInvMass(),
-				m_rbB.getInvInertiaDiagLocal(),
-				m_rbB.getInvMass());
-		}
-	}
-
-	btVector3 b1Axis1,b1Axis2,b1Axis3;
-	btVector3 b2Axis1,b2Axis2;
-
-	b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
-	b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
-
-	btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
-
-	btScalar swx=btScalar(0.),swy = btScalar(0.);
-	btScalar thresh = btScalar(10.);
-	btScalar fact;
-
-	// Get Frame into world space
-	if (m_swingSpan1 >= btScalar(0.05f))
-	{
-		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);
-		swing1  = btAtan2Fast(swy, swx);
-		fact = (swy*swy + swx*swx) * thresh * thresh;
-		fact = fact / (fact + btScalar(1.0));
-		swing1 *= fact; 
-
-	}
-
-	if (m_swingSpan2 >= btScalar(0.05f))
-	{
-		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);
-		swing2  = btAtan2Fast(swy, swx);
-		fact = (swy*swy + swx*swx) * thresh * thresh;
-		fact = fact / (fact + btScalar(1.0));
-		swing2 *= fact; 
-	}
-
-	btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);		
-	btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);	
-	btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
-
-	if (EllipseAngle > 1.0f)
-	{
-		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));
-
-	}
-
-	// Twist limits
-	if (m_twistSpan >= btScalar(0.))
-	{
-		btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
-		btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
-		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.);
-		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)
+			btVector3 normal[3];
+			if (relPos.length2() > SIMD_EPSILON)
 			{
-				m_twistCorrection = (twist - m_twistSpan);
+				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(
+					pivotAInW - m_rbA.getCenterOfMassPosition(),
+					pivotBInW - m_rbB.getCenterOfMassPosition(),
+					normal[i],
+					m_rbA.getInvInertiaDiagLocal(),
+					m_rbA.getInvMass(),
+					m_rbB.getInvInertiaDiagLocal(),
+					m_rbB.getInvMass());
+			}
+		}
+
+		btVector3 b1Axis1,b1Axis2,b1Axis3;
+		btVector3 b2Axis1,b2Axis2;
+
+		b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
+		b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
+
+		btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
+
+		btScalar swx=btScalar(0.),swy = btScalar(0.);
+		btScalar thresh = btScalar(10.);
+		btScalar fact;
+
+		// Get Frame into world space
+		if (m_swingSpan1 >= btScalar(0.05f))
+		{
+			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);
+			swing1  = btAtan2Fast(swy, swx);
+			fact = (swy*swy + swx*swx) * thresh * thresh;
+			fact = fact / (fact + btScalar(1.0));
+			swing1 *= fact; 
+
+		}
+
+		if (m_swingSpan2 >= btScalar(0.05f))
+		{
+			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);
+			swing2  = btAtan2Fast(swy, swx);
+			fact = (swy*swy + swx*swx) * thresh * thresh;
+			fact = fact / (fact + btScalar(1.0));
+			swing2 *= fact; 
+		}
+
+		btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);		
+		btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);	
+		btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
+
+		if (EllipseAngle > 1.0f)
+		{
+			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));
+
+		}
+
+		// Twist limits
+		if (m_twistSpan >= btScalar(0.))
+		{
+			btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
+			btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
+			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.);
+			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);
+					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)
+void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,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)
+	if (m_useSolveConstraintObsolete)
 	{
-		btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
-		btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
 
-		btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-		btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-		btVector3 vel = vel1 - vel2;
+		btScalar tau = btScalar(0.3);
 
-		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)
+		//linear part
+		if (!m_angularOnly)
 		{
-			btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
-			btScalar impulseMag = amplitude * m_kSwing;
+			btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+			btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
 
-			// Clamp the accumulated impulse
-			btScalar temp = m_accSwingLimitImpulse;
-			m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
-			impulseMag = m_accSwingLimitImpulse - temp;
+			btVector3 vel1;
+			bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+			btVector3 vel2;
+			bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+			btVector3 vel = vel1 - vel2;
 
-			btVector3 impulse = m_swingAxis * impulseMag;
-
-			m_rbA.applyTorqueImpulse(impulse);
-			m_rbB.applyTorqueImpulse(-impulse);
+			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);
+		
+			}
 		}
-
-		// 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;
+			///solve angular part
+			btVector3 angVelA;
+			bodyA.getAngularVelocity(angVelA);
+			btVector3 angVelB;
+			bodyB.getAngularVelocity(angVelB);
 
-			// Clamp the accumulated impulse
-			btScalar temp = m_accTwistLimitImpulse;
-			m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
-			impulseMag = m_accTwistLimitImpulse - temp;
+			// 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;
 
-			btVector3 impulse = m_twistAxis * impulseMag;
+				// Clamp the accumulated impulse
+				btScalar temp = m_accSwingLimitImpulse;
+				m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
+				impulseMag = m_accSwingLimitImpulse - temp;
 
-			m_rbA.applyTorqueImpulse(impulse);
-			m_rbB.applyTorqueImpulse(-impulse);
+				btVector3 impulse = m_swingAxis * impulseMag;
 
+				bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_swingAxis,impulseMag);
+				bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_swingAxis,-impulseMag);
+			}
+
+			// 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;
+
+				bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
+				bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
+
+			}
+		
 		}
-	
 	}
 
 }
diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
index ac3353a86..b91522480 100644
--- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
@@ -63,6 +63,7 @@ public:
 	bool		m_solveTwistLimit;
 	bool		m_solveSwingLimit;
 
+	bool	m_useSolveConstraintObsolete;
 	
 public:
 
@@ -74,7 +75,12 @@ public:
 
 	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);
 
diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
index 11715708d..30b310d07 100644
--- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
+++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
@@ -22,7 +22,9 @@ enum	btSolverMode
 	SOLVER_FRICTION_SEPARATE = 2,
 	SOLVER_USE_WARMSTARTING = 4,
 	SOLVER_USE_FRICTION_WARMSTARTING = 8,
-	SOLVER_CACHE_FRIENDLY = 16
+	SOLVER_CACHE_FRIENDLY = 16,
+	SOLVER_SIMD = 32,//enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version
+	SOLVER_CUDA = 64 //will be open sourced during Game Developers Conference 2009. Much faster.
 };
 
 struct btContactSolverInfoData
@@ -72,7 +74,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		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
 	}
 };
diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
index abcb8d753..1fc8dce3c 100644
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
@@ -26,6 +26,35 @@ http://gimpact.sf.net
 #include <new>
 
 
+btGeneric6DofConstraint::btGeneric6DofConstraint()
+:btTypedConstraint(D6_CONSTRAINT_TYPE),
+m_useLinearReferenceFrameA(true),
+m_useSolveConstraintObsolete(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),
+m_useSolveConstraintObsolete(true)
+{
+
+}
+
+
+
+#define dCROSSMAT(A,a,skip,plus,minus) \
+{ \
+  (A)[1] = minus (a)[2]; \
+  (A)[2] = plus (a)[1]; \
+  (A)[(skip)+0] = plus (a)[2]; \
+  (A)[(skip)+2] = minus (a)[0]; \
+  (A)[2*(skip)+0] = minus (a)[1]; \
+  (A)[2*(skip)+1] = plus (a)[0]; \
+} 
+
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
 
 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
@@ -40,37 +69,37 @@ btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
 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
-//
+	//	// 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))
+	{
+		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;
-			}
+			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;
@@ -104,85 +133,89 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value)
 
 	m_currentLimit = 0;//Free from violation
 	return 0;
-	
+
 }
 
 
 btScalar btRotationalLimitMotor::solveAngularLimits(
-		btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
-	 	btRigidBody * body0, btRigidBody * body1)
+	btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
+	btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
 {
-    if (needApplyTorques()==false) return 0.0f;
+	if (needApplyTorques()==false) return 0.0f;
 
-    btScalar target_velocity = m_targetVelocity;
-    btScalar maxMotorForce = m_maxMotorForce;
+	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;
-    }
+	if (m_currentLimit!=0)
+	{
+		target_velocity = -m_ERP*m_currentLimitError/(timeStep);
+		maxMotorForce = m_maxLimitForce;
+	}
 
-    maxMotorForce *= timeStep;
+	maxMotorForce *= timeStep;
 
-    // current velocity difference
-    btVector3 vel_diff = body0->getAngularVelocity();
-    if (body1)
-    {
-        vel_diff -= body1->getAngularVelocity();
-    }
+	// 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);
+	btScalar rel_vel = axis.dot(vel_diff);
 
 	// correction velocity
-    btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
+	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
-    }
+	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;
+	btScalar clippedMotorImpulse;
 
-    ///@todo: should clip against accumulated impulse
-    if (unclippedMotorImpulse>0.0f)
-    {
-        clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
-    }
-    else
-    {
-        clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
-    }
+	///@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	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;
+	btScalar oldaccumImpulse = m_accumulatedImpulse;
+	btScalar sum = oldaccumImpulse + clippedMotorImpulse;
+	m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
 
-    clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
+	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);
 
 
-
-    btVector3 motorImp = clippedMotorImpulse * axis;
-
-
-    body0->applyTorqueImpulse(motorImp);
-    if (body1) body1->applyTorqueImpulse(-motorImp);
-
-    return clippedMotorImpulse;
+	return clippedMotorImpulse;
 
 
 }
@@ -191,98 +224,94 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
 
 //////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
 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)
+	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();
+	///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;
+	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);
+	btScalar rel_vel = axis_normal_on_a.dot(vel);
 
 
 
-/// apply displacement correction
+	/// 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);
+	//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];
+	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.);
+	//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;
-                }
-            }
-        }
-    }
+			}
+			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 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;
+	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;
+	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)
-{
-
-}
-
 
 
 
@@ -296,19 +325,19 @@ void btGeneric6DofConstraint::calculateAngleInfo()
 
 
 	// 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,189 +347,459 @@ void btGeneric6DofConstraint::calculateAngleInfo()
 	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);
-//    }
+	//    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);
+	//    }
 
 }
 
 void btGeneric6DofConstraint::calculateTransforms()
 {
-    m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
-    m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
+	m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
+	m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
 
-    calculateAngleInfo();
+	calculateAngleInfo();
 }
 
 
 void btGeneric6DofConstraint::buildLinearJacobian(
-    btJacobianEntry & jacLinear,const btVector3 & normalWorld,
-    const btVector3 & pivotAInW,const btVector3 & pivotBInW)
+	btJacobianEntry & jacLinear,const btVector3 & normalWorld,
+	const btVector3 & pivotAInW,const btVector3 & pivotBInW)
 {
-    new (&jacLinear) btJacobianEntry(
+	new (&jacLinear) btJacobianEntry(
 
-        pivotAInW - m_rbA.getCenterOfMassPosition(),
-        pivotBInW - m_rbB.getCenterOfMassPosition(),
-        normalWorld,
-        m_rbA.getInvInertiaDiagLocal(),
-        m_rbA.getInvMass(),
-        m_rbB.getInvInertiaDiagLocal(),
-        m_rbB.getInvMass());
+		pivotAInW - m_rbA.getCenterOfMassPosition(),
+		pivotBInW - m_rbB.getCenterOfMassPosition(),
+		normalWorld,
+		m_rbA.getInvInertiaDiagLocal(),
+		m_rbA.getInvMass(),
+		m_rbB.getInvInertiaDiagLocal(),
+		m_rbB.getInvMass());
 
 }
 
 void btGeneric6DofConstraint::buildAngularJacobian(
-    btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
+	btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
 {
-    new (&jacAngular)	btJacobianEntry(jointAxisW,
-                                      m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-                                      m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-                                      m_rbA.getInvInertiaDiagLocal(),
-                                      m_rbB.getInvInertiaDiagLocal());
+	new (&jacAngular)	btJacobianEntry(jointAxisW,
+		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+		m_rbA.getInvInertiaDiagLocal(),
+		m_rbB.getInvInertiaDiagLocal());
 
 }
 
 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
 {
-    btScalar angle = m_calculatedAxisAngleDiff[axis_index];
+	btScalar angle = m_calculatedAxisAngleDiff[axis_index];
 
-    //test limits
-    m_angularLimits[axis_index].testLimitValue(angle);
-    return m_angularLimits[axis_index].needApplyTorques();
+	//test limits
+	m_angularLimits[axis_index].testLimitValue(angle);
+	return m_angularLimits[axis_index].needApplyTorques();
 }
 
 void btGeneric6DofConstraint::buildJacobian()
 {
+	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();
+		// 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;
+		//  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();
+		// 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);
+		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);
+				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);
-        }
-    }
+		// 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 = 3;
+		info->nub = 3;//??
+		//test angular limits
+		for (int i=0;i<3 ;i++ )
+		{
+			//if(i==2) continue;
+			if(testAngularLimitMotor(i))
+			{
+				info->m_numConstraintRows++;
+			}
+		}
+	}
+}
+
+void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	btAssert(!m_useSolveConstraintObsolete);
+
+	int row = setLinearLimits(info);
+	setAngularLimits(info, row);
+}
+
+int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
+{
+
+	btGeneric6DofConstraint * d6constraint = this;
+
+	//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.
+
+	int s = info->rowskip;
+
+	// set jacobian
+	info->m_J1linearAxis[0] = 1;
+	info->m_J1linearAxis[s+1] = 1;
+	info->m_J1linearAxis[2*s+2] = 1;
+
+
+	btVector3 a1,a2;
+
+	a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin();
+	//dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
+	dCROSSMAT (info->m_J1angularAxis,a1,s,-,+);
+
+	/*info->m_J2linearAxis[0] = -1;
+	info->m_J2linearAxis[s+1] = -1;
+	info->m_J2linearAxis[2*s+2] = -1;
+	*/
+
+	a2 = body1_trans.getBasis()*d6constraint->getFrameOffsetB().getOrigin();
+	//dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB);
+	dCROSSMAT (info->m_J2angularAxis,a2,s,+,-);
+
+	// set right hand side
+	btScalar k = info->fps * info->erp;
+	for (int j=0; j<3; j++)
+	{
+		info->m_constraintError[s*j] = k * (a2[j] + body1_trans.getOrigin()[j] -
+			a1[j] - body0_trans.getOrigin()[j]);
+	}
+
+	return 3;
 
 }
 
 
-void btGeneric6DofConstraint::solveConstraint(btScalar	timeStep)
+/*! \pre testLimitValue must be called on limot*/
+int bt_get_limit_motor_info2(
+	btRotationalLimitMotor * limot,
+	btRigidBody * body0, btRigidBody * body1,
+	btTypedConstraint::btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
 {
-    m_timeStep = timeStep;
 
-    //calculateTransforms();
 
-    int i;
+    int srow = row * info->rowskip;
 
-    // linear
+    // if the joint is powered, or has joint limits, add in the extra row
+    int powered = limot->m_enableMotor;
+    int limit = limot->m_currentLimit;
 
-    btVector3 pointInA = m_calculatedTransformA.getOrigin();
-	btVector3 pointInB = m_calculatedTransformB.getOrigin();
-
-	btScalar jacDiagABInv;
-	btVector3 linear_axis;
-    for (i=0;i<3;i++)
+    if (powered || limit)
     {
-        if (m_linearLimits.isLimited(i))
+        btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
+        btScalar *J2 = rotational ? info->m_J2angularAxis : 0;//info->m_J2linearAxis;
+
+        J1[srow+0] = ax1[0];
+        J1[srow+1] = ax1[1];
+        J1[srow+2] = ax1[2];
+        if (body1)
         {
-            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);
-
+            J2[srow+0] = -ax1[0];
+            J2[srow+1] = -ax1[1];
+            J2[srow+2] = -ax1[2];
         }
-    }
 
-    // angular
-    btVector3 angular_axis;
-    btScalar angularJacDiagABInv;
-    for (i=0;i<3;i++)
-    {
-        if (m_angularLimits[i].needApplyTorques())
+        // linear limot torque decoupling step:
+        //
+        // if this is a linear limot (e.g. from a slider), 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 powered or limited slider-jointed free
+        // bodies from gaining angular momentum.
+        // the solution used here is to apply the constraint forces at the point
+        // halfway between the body centers. there is no penalty (other than an
+        // extra tiny bit of computation) in doing this adjustment. note that we
+        // only need to do this if the constraint connects two bodies.
+
+        btVector3 ltd;	// Linear Torque Decoupling vector (a torque)
+        if (!rotational && body1)
         {
+            btVector3 c;
+            c[0]=btScalar(0.5)*(body1->getCenterOfMassPosition()[0]
+            				-body0->getCenterOfMassPosition()[0]);
+            c[1]=btScalar(0.5)*(body1->getCenterOfMassPosition()[1]
+            				-body0->getCenterOfMassPosition()[1]);
+            c[2]=btScalar(0.5)*(body1->getCenterOfMassPosition()[2]
+            				-body0->getCenterOfMassPosition()[2]);
 
-			// get axis
-			angular_axis = getAxis(i);
+			ltd = c.cross(ax1);
 
-			angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
-
-			m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,&m_rbB);
+            info->m_J1angularAxis[srow+0] = ltd[0];
+            info->m_J1angularAxis[srow+1] = ltd[1];
+            info->m_J1angularAxis[srow+2] = ltd[2];
+            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;
+
+        if (powered)
+        {
+            info->cfm[srow] = 0.0f;//limot->m_normalCFM;
+            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;
+            info->m_constraintError[srow] = -k * limot->m_currentLimitError;
+            info->cfm[srow] = 0.0f;//limot->m_stopCFM;
+
+            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)
+                {
+                    // 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;
+                }
+
+                // 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)
+                    {
+                        // low limit
+                        if (vel < 0)
+                        {
+                            btScalar newc = -limot->m_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 = -limot->m_bounce * vel;
+                            if (newc < info->m_constraintError[srow]) 
+								info->m_constraintError[srow] = newc;
+                        }
+                    }
+                }
+            }
+        }
+        return 1;
     }
+    else return 0;
+}
+
+
+
+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(i==2) continue;
+		if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
+		{
+			btVector3 axis = d6constraint->getAxis(i);
+			row += bt_get_limit_motor_info2(
+				d6constraint->getRotationalLimitMotor(i),
+				&m_rbA,
+				&m_rbB,
+				info,row,axis,1);
+		}
+	}
+
+	return row;
+}
+
+///////////////////limit motor support
+
+
+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)
diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
index f0718d2d4..875d108b2 100644
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
@@ -110,7 +110,7 @@ public:
 	int testLimitValue(btScalar test_value);
 
 	//! 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);
 
 
 };
@@ -168,8 +168,8 @@ public:
     btScalar solveLinearAxis(
     	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,
 		const btVector3 & anchorPos);
@@ -262,6 +262,9 @@ protected:
     }
 
 
+	int setAngularLimits(btConstraintInfo2 *info, int row_offset);
+
+	int setLinearLimits(btConstraintInfo2 *info);
 
     void buildLinearJacobian(
         btJacobianEntry & jacLinear,const btVector3 & normalWorld,
@@ -276,6 +279,10 @@ protected:
 
 
 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);
 
     btGeneric6DofConstraint();
@@ -330,7 +337,11 @@ public:
 	//! performs Jacobian calculation, and also calculates angle differences and axis
     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);
 
diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
index ae8ba96a7..5f576f12a 100644
--- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
@@ -19,11 +19,14 @@ subject to the following restrictions:
 #include "LinearMath/btTransformUtil.h"
 #include "LinearMath/btMinMax.h"
 #include <new>
+#include "btSolverBody.h"
+
 
 
 btHingeConstraint::btHingeConstraint()
 : btTypedConstraint (HINGE_CONSTRAINT_TYPE),
-m_enableAngularMotor(false)
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(true)
 {
 }
 
@@ -31,7 +34,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
 									 btVector3& axisInA,btVector3& axisInB)
 									 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
 									 m_angularOnly(false),
-									 m_enableAngularMotor(false)
+									 m_enableAngularMotor(false),
+									 m_useSolveConstraintObsolete(true)
 {
 	m_rbAFrame.getOrigin() = pivotInA;
 	
@@ -77,7 +81,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
 
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
-:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(true)
 {
 
 	// since no frame is given, assume this to be zero angle and just pick rb transform axis
@@ -115,7 +119,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
 								     const btTransform& rbAFrame, const btTransform& rbBFrame)
 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
 m_angularOnly(false),
-m_enableAngularMotor(false)
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(true)
 {
 	// flip axis
 	m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
@@ -136,7 +141,8 @@ m_enableAngularMotor(false)
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
 m_angularOnly(false),
-m_enableAngularMotor(false)
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(true)
 {
 	///not providing rigidbody B means implicitly using worldspace for body B
 
@@ -158,226 +164,270 @@ m_enableAngularMotor(false)
 
 void	btHingeConstraint::buildJacobian()
 {
-	m_appliedImpulse = btScalar(0.);
-
-	if (!m_angularOnly)
+	if (m_useSolveConstraintObsolete)
 	{
-		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
-		btVector3 relPos = pivotBInW - pivotAInW;
+		m_appliedImpulse = btScalar(0.);
 
-		btVector3 normal[3];
-		if (relPos.length2() > SIMD_EPSILON)
+		if (!m_angularOnly)
 		{
-			normal[0] = relPos.normalized();
-		}
-		else
-		{
-			normal[0].setValue(btScalar(1.0),0,0);
+			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(
+					pivotAInW - m_rbA.getCenterOfMassPosition(),
+					pivotBInW - m_rbB.getCenterOfMassPosition(),
+					normal[i],
+					m_rbA.getInvInertiaDiagLocal(),
+					m_rbA.getInvMass(),
+					m_rbB.getInvInertiaDiagLocal(),
+					m_rbB.getInvMass());
+			}
 		}
 
-		btPlaneSpace1(normal[0], normal[1], normal[2]);
+		//calculate two perpendicular jointAxis, orthogonal to hingeAxis
+		//these two jointAxis require equal angular velocities for both bodies
 
-		for (int i=0;i<3;i++)
-		{
-			new (&m_jac[i]) btJacobianEntry(
-				pivotAInW - m_rbA.getCenterOfMassPosition(),
-				pivotBInW - m_rbB.getCenterOfMassPosition(),
-				normal[i],
-				m_rbA.getInvInertiaDiagLocal(),
-				m_rbA.getInvMass(),
-				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);
+		//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());
+		btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
 
-	new (&m_jacAng[1])	btJacobianEntry(jointAxis1,
-		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-		m_rbA.getInvInertiaDiagLocal(),
-		m_rbB.getInvInertiaDiagLocal());
+		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[2])	btJacobianEntry(hingeAxisWorld,
-		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());
 
 
-	// Compute limit information
-	btScalar hingeAngle = getHingeAngle();  
+		// Compute limit information
+		btScalar hingeAngle = getHingeAngle();  
 
-	//set bias, sign, clear accumulator
-	m_correction = btScalar(0.);
-	m_limitSign = btScalar(0.);
-	m_solveLimit = false;
-	m_accLimitImpulse = btScalar(0.);
+		//set bias, sign, clear accumulator
+		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)
+	//	if (m_lowerLimit < m_upperLimit)
+		if (m_lowerLimit <= m_upperLimit)
 		{
-			m_correction = (m_lowerLimit - 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;
-			m_limitSign = -1.0f;
-			m_solveLimit = true;
+	//		if (hingeAngle <= m_lowerLimit*m_limitSoftness)
+			if (hingeAngle <= m_lowerLimit)
+			{
+				m_correction = (m_lowerLimit - 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;
+				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));
+
 	}
-
-	//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)
+
+void btHingeConstraint::getInfo1 (btConstraintInfo1* info)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
+	} else
+	{
+		btAssert(0);
+	}
+}
+
+void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	btAssert(0);
+}
+
+
+void	btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,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)
+	///for backwards compatibility during the transition to 'getInfo/getInfo2'
+	if (m_useSolveConstraintObsolete)
 	{
-		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;
+		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
 
-		for (int i=0;i<3;i++)
-		{		
-			const btVector3& normal = m_jac[i].m_linearJointAxis;
-			btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+		btScalar tau = btScalar(0.3);
 
-			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;
+		//linear part
+		if (!m_angularOnly)
 		{
-			//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;
-			}
+			btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+			btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
 
-			//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;
-			}
+			btVector3 vel1,vel2;
+			bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+			bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+			btVector3 vel = vel1 - vel2;
 
-			m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
-			m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
+			for (int i=0;i<3;i++)
+			{		
+				const btVector3& normal = m_jac[i].m_linearJointAxis;
+				btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
 
-			// 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);
+				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);
 			}
 		}
 
-		//apply motor
-		if (m_enableAngularMotor) 
+		
 		{
-			//todo: add limits too
-			btVector3 angularLimit(0,0,0);
+			///solve angular part
 
-			btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
-			btScalar projRelVel = velrel.dot(axisA);
+			// 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);
 
-			btScalar desiredMotorVel = m_motorTargetVelocity;
-			btScalar motor_relvel = desiredMotorVel - projRelVel;
+			btVector3 angVelA;
+			bodyA.getAngularVelocity(angVelA);
+			btVector3 angVelB;
+			bodyB.getAngularVelocity(angVelB);
 
-			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;
+			btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
+			btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
 
-			m_rbA.applyTorqueImpulse(motorImp+angularLimit);
-			m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
+			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);
+				
+			}
 		}
 	}
 
diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
index 4fa9972f6..6405381c6 100644
--- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
@@ -57,6 +57,7 @@ public:
 	bool		m_angularOnly;
 	bool		m_enableAngularMotor;
 	bool		m_solveLimit;
+	bool	m_useSolveConstraintObsolete;
 
 	
 public:
@@ -73,7 +74,11 @@ public:
 
 	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);
 
diff --git a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
index 5046328a2..03ca2334b 100644
--- a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
@@ -21,100 +21,197 @@ subject to the following restrictions:
 
 
 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)
 {
 
 }
 
 
 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)
 {
 	
 }
 
 void	btPoint2PointConstraint::buildJacobian()
 {
-	m_appliedImpulse = btScalar(0.);
-
-	btVector3	normal(0,0,0);
-
-	for (int i=0;i<3;i++)
+	if (m_useSolveConstraintObsolete)
 	{
-		normal[i] = 1;
-		new (&m_jac[i]) btJacobianEntry(
+		m_appliedImpulse = btScalar(0.);
 
-			m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
-			m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
-			normal,
-			m_rbA.getInvInertiaDiagLocal(),
-			m_rbA.getInvMass(),
-			m_rbB.getInvInertiaDiagLocal(),
-			m_rbB.getInvMass());
-		normal[i] = 0;
+		btVector3	normal(0,0,0);
+
+		for (int i=0;i<3;i++)
+		{
+			normal[i] = 1;
+			new (&m_jac[i]) btJacobianEntry(
+
+				m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
+				m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
+				normal,
+				m_rbA.getInvInertiaDiagLocal(),
+				m_rbA.getInvMass(),
+				m_rbB.getInvInertiaDiagLocal(),
+				m_rbB.getInvMass());
+			normal[i] = 0;
+		}
 	}
 
 }
 
-void	btPoint2PointConstraint::solveConstraint(btScalar	timeStep)
+
+void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
 {
-	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
-	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
+	if (m_useSolveConstraintObsolete)
+	{
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
+	} else
+	{
+		info->m_numConstraintRows = 3;
+		info->nub = 3;
+	}
+}
+#define dCROSSMAT(A,a,skip,plus,minus) \
+{ \
+  (A)[1] = minus (a)[2]; \
+  (A)[2] = plus (a)[1]; \
+  (A)[(skip)+0] = plus (a)[2]; \
+  (A)[(skip)+2] = minus (a)[0]; \
+  (A)[2*(skip)+0] = minus (a)[1]; \
+  (A)[2*(skip)+1] = plus (a)[0]; \
+} 
+#include <stdio.h>
 
+void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	btAssert(!m_useSolveConstraintObsolete);
 
-	btVector3 normal(0,0,0);
-	
+	 //retrieve matrices
+	btTransform body0_trans;
+	body0_trans = m_rbA.getCenterOfMassTransform();
+    btTransform body1_trans;
+	body1_trans = m_rbB.getCenterOfMassTransform();
 
-//	btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-//	btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
+	// anchor points in global coordinates with respect to body PORs.
+    int s = info->rowskip;
 
-	for (int i=0;i<3;i++)
-	{		
-		normal[i] = 1;
-		btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+    // set jacobian
+    info->m_J1linearAxis[0] = 1;
+    info->m_J1linearAxis[s+1] = 1;
+    info->m_J1linearAxis[2*s+2] = 1;
 
-		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);
+    btVector3 a1,a2;
 
-	/*
-		//velocity error (first order error)
-		btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
-														m_rbB.getLinearVelocity(),angvelB);
+    a1 = body0_trans.getBasis()*getPivotInA();
+    //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
+    dCROSSMAT (info->m_J1angularAxis,a1,s,-,+);
+    
+	/*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
-		
-		btScalar impulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
 
-		btScalar impulseClamp = m_setting.m_impulseClamp;
+    a2 = body1_trans.getBasis()*getPivotInB();
+	//dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB);
+    //dCROSSMAT (info->m_J2angularAxis,a2,s,+,-);
+	dCROSSMAT (info->m_J2angularAxis,a2,s,+,-);
+
+    
+
+    // set right hand side
+    btScalar k = info->fps * info->erp;
+    int j;
+
+	for (j=0; j<3; j++)
+    {
+        info->m_constraintError[j*s] = 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 (impulseClamp > 0)
 		{
-			if (impulse < -impulseClamp) 
-				impulse = -impulseClamp;
-			if (impulse > impulseClamp) 
-				impulse = impulseClamp;
+			info->m_lowerLimit[j*s] = -impulseClamp;
+			info->m_upperLimit[j*s] = impulseClamp;
 		}
+	}
+	
+}
 
-		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());
+
+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);
 		
-		normal[i] = 0;
+
+	//	btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
+	//	btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
+
+		for (int i=0;i<3;i++)
+		{		
+			normal[i] = 1;
+			btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+
+			btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+			btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+			//this jacobian entry could be re-used for all iterations
+			
+			btVector3 vel1,vel2;
+			bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+			bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+			btVector3 vel = vel1 - vel2;
+			
+			btScalar rel_vel;
+			rel_vel = normal.dot(vel);
+
+		/*
+			//velocity error (first order error)
+			btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
+															m_rbB.getLinearVelocity(),angvelB);
+		*/
+		
+			//positional error (zeroth order error)
+			btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+			
+			btScalar 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;
+			}
+
+			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);
+
+
+			normal[i] = 0;
+		}
 	}
 }
 
diff --git a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
index c9d596853..e2b865cd4 100644
--- a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
@@ -50,6 +50,9 @@ public:
 	
 public:
 
+	///for backwards compatibility during the transition to 'getInfo/getInfo2'
+	bool		m_useSolveConstraintObsolete;
+
 	btConstraintSetting	m_setting;
 
 	btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB);
@@ -60,8 +63,12 @@ public:
 
 	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);
 
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index 381592d79..78209a11b 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -37,29 +37,73 @@ subject to the following restrictions:
 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
 :m_btSeed2(0)
 {
+
 }
 
 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
 {
 }
 
-// Project Gauss Seidel or the equivalent Sequential Impulse
-void btSequentialImpulseConstraintSolver::resolveSingleConstraintRow(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+#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 )
 {
-	float deltaImpulse;
-	deltaImpulse = c.m_rhs-c.m_appliedImpulse*c.m_cfm;
-	btScalar deltaVel1Dotn	=	c.m_contactNormal.dot(body1.m_deltaLinearVelocity) 	+ c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
-	btScalar deltaVel2Dotn	=	c.m_contactNormal.dot(body2.m_deltaLinearVelocity) 	+ c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
-	btScalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;
+    __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
+SIMD_FORCE_INLINE 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_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body2.m_deltaLinearVelocity.mVec128) ,_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128));
+	__m128 delta_rel_vel	=	_mm_sub_ps(deltaVel1Dotn,deltaVel2Dotn);
+	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+	deltaImpulse	=	_mm_add_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_sub_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
+SIMD_FORCE_INLINE 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;
-
-	btScalar sum = c.m_appliedImpulse + deltaImpulse;
+	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;
@@ -69,8 +113,71 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRow(btSolverBod
 	{
 		c.m_appliedImpulse = sum;
 	}
-	body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
-	body2.applyImpulse(c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,-deltaImpulse);
+	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);
+}
+
+
+SIMD_FORCE_INLINE 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 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_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body2.m_deltaLinearVelocity.mVec128) ,_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128));
+	__m128 delta_rel_vel	=	_mm_sub_ps(deltaVel1Dotn,deltaVel2Dotn);
+	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+	deltaImpulse	=	_mm_add_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);
+	__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));
+	//body1.applyImpulse(c.m_contactNormal*body1.m_invMass.m_vec128,c.m_angularComponentA,deltaImpulse);
+	//body2.applyImpulse(c.m_contactNormal*body2.m_invMass.m_vec128,c.m_angularComponentB,-deltaImpulse);
+	__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_sub_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
+SIMD_FORCE_INLINE 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);
+	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
+	{
+		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);
 }
 
 
@@ -113,39 +220,24 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
 
 
 
-
-
-
-
-
 void	btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
 {
-	btRigidBody* rb = btRigidBody::upcast(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,149 +249,19 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel,
 	return rest;
 }
 
-//SIMD_FORCE_INLINE
-void	btSequentialImpulseConstraintSolver::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);
-               
-        }
-
-}
-
-
-
-
-
-//SIMD_FORCE_INLINE 
-void btSequentialImpulseConstraintSolver::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;
-		{
-
-#ifndef _USE_JACOBIAN
-			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;
-#else
-			btScalar rel_vel = contactConstraint.m_jac.getRelativeVelocity(body1.m_linearVelocity1+body1.m_deltaLinearVelocity,body1.m_angularVelocity1+body1.m_deltaAngularVelocity,
-			body2.m_linearVelocity1+body2.m_deltaLinearVelocity,body2.m_angularVelocity1+body2.m_deltaAngularVelocity);
-#endif //_USE_JACOBIAN
-
-			// 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.applyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
-		
-		body2.applyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
-
-	} 
-}
 
 
 
 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;
@@ -310,8 +272,8 @@ btSolverConstraint&	btSequentialImpulseConstraintSolver::addFrictionConstraint(c
 	solverConstraint.m_friction = cp.m_combinedFriction;
 	solverConstraint.m_originalContactPoint = 0;
 
-	solverConstraint.m_appliedImpulse = btScalar(0.);
-	solverConstraint.m_appliedPushImpulse = 0.f;
+	solverConstraint.m_appliedImpulse = 0.f;
+//	solverConstraint.m_appliedPushImpulse = 0.f;
 	solverConstraint.m_penetration = 0.f;
 	{
 		btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
@@ -355,10 +317,57 @@ btSolverConstraint&	btSequentialImpulseConstraintSolver::addFrictionConstraint(c
 								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;
+		positionalError = 0;//-solverConstraint.m_penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
+		solverConstraint.m_restitution=0.f;
+
+		btSimdScalar velocityError = solverConstraint.m_restitution - 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>
 
 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
 {
@@ -372,16 +381,149 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 //		printf("empty\n");
 		return 0.f;
 	}
-	btPersistentManifold* manifold = 0;
-	btCollisionObject* colObj0=0,*colObj1=0;
+	
+	btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+	initSolverBody(&fixedBody,0);
 
 	//btRigidBody* rb0=0,*rb1=0;
 
 	//if (1)
 	{
+		{
+			
+			int totalNumRows = 0;
+			
+			//calculate the total number of contraint rows
+			for (int 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;
+			
+			if (m_tmpSolverNonContactConstraintPool.size()>3)
+			{
+				printf("dsadas\n");
+			}
+			///setup the btSolverConstraints
+			int currentRow = 0;
+
+			for (int 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];
+
+					for (int 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_penetration = 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
+					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 (int j=0;j<info1.m_numConstraintRows;j++)
+					{
+						btSolverConstraint& solverConstraint = currentConstraintRow[j];
+
+						{
+							const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
+							solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1;
+						}
+						{
+							const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
+							solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2;
+						}
+
+						{
+							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 = 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 positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
+							solverConstraint.m_restitution  = 0.f;
+							btScalar	velocityError = solverConstraint.m_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++)
 			{
@@ -394,49 +536,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 				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);
-					}
+					solverBodyIdA = getOrInitSolverBody(*colObj0);
+					solverBodyIdB = getOrInitSolverBody(*colObj1);
 				}
 
 				btVector3 rel_pos1;
@@ -463,10 +564,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 						btScalar rel_vel;
 						btVector3 vel;
 
-						int frictionIndex = m_tmpSolverConstraintPool.size();
+						int frictionIndex = m_tmpSolverContactConstraintPool.size();
 
 						{
-							btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
+							btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
 							btRigidBody* rb0 = btRigidBody::upcast(colObj0);
 							btRigidBody* rb1 = btRigidBody::upcast(colObj1);
 
@@ -541,7 +642,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 								};
 							}
 
-							
+
 							///warm starting (or zero if disabled)
 							if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 							{
@@ -555,7 +656,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 								solverConstraint.m_appliedImpulse = 0.f;
 							}
 
-							solverConstraint.m_appliedPushImpulse = 0.f;
+//							solverConstraint.m_appliedPushImpulse = 0.f;
 							
 							{
 								btScalar rel_vel;
@@ -568,14 +669,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 								btScalar positionalError = 0.f;
 								positionalError = -solverConstraint.m_penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
-								btScalar velocityError = solverConstraint.m_restitution - rel_vel;// * damping;
-
-								btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
-								btScalar	velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
-								solverConstraint.m_rhs = (penetrationImpulse+velocityImpulse);
+								btScalar	velocityError = solverConstraint.m_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 = 1e30f;
+								solverConstraint.m_upperLimit = 1e10f;
 							}
 
 
@@ -594,7 +694,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 							if (1)
 							{
-							solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size();
+							solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
 							if (!cp.m_lateralFrictionInitialized)
 							{
 								cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
@@ -615,9 +715,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 									//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)
 									{
+										addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 										cp.m_lateralFrictionInitialized = true;
 									}
 								}
@@ -632,7 +732,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 							if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
 							{
 								{
-									btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
+									btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
 									if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 									{
 										frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
@@ -646,7 +746,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 									}
 								}
 								{
-									btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
+									btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
 									if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 									{
 										frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
@@ -672,6 +772,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 	
 	btContactSolverInfo info = infoGlobal;
 
+	if (1)
 	{
 		int j;
 		for (j=0;j<numConstraints;j++)
@@ -681,8 +782,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 		}
 	}
 	
-	int numConstraintPool = m_tmpSolverConstraintPool.size();
-	int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
+	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
 
 	///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
 	m_orderTmpConstraintPool.resize(numConstraintPool);
@@ -706,8 +807,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
 {
 	BT_PROFILE("solveGroupCacheFriendlyIterations");
-	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...
 	int iteration;
@@ -735,82 +837,96 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
 				}
 			}
 
-			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))
+				///solve all joint constraints, using SIMD, if available
+				for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
 				{
-					m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
-				}
-				if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
-				{
-					m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
+					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+					resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
 				}
 
-				constraint->solveConstraint(infoGlobal.m_timeStep);
-
-				if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
+				for (j=0;j<numConstraints;j++)
 				{
-					m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
-				}
-				if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
-				{
-					m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
+					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);
 				}
 
-			}
-
-
-			{
-				int numPoolConstraints = m_tmpSolverConstraintPool.size();
+				///solve all contact constraints using SIMD, if available
+				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
 				for (j=0;j<numPoolConstraints;j++)
 				{
-					const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
-					resolveSingleConstraintRow(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+					resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 				}
-			}
-
-			{
-				 int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
+				///solve all friction constraints, using SIMD, if available
+				 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.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);
-				}
-			}
-			
-
-
-		}
-	
-		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++)
+					btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+					
+					if (totalImpulse>btScalar(0))
 					{
-						const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
+						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
 
-						resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
-							m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
+						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);
 					}
 				}
 			}
+			
+
 
 		}
-
 	}
-
 	return 0.f;
 }
 
@@ -830,23 +946,23 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
 	solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
 	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);
 		pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
 		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?
-
 	}
 
 	if (infoGlobal.m_splitImpulse)
@@ -865,8 +981,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
 
 
 	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;
 }
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
index fb16fdef2..f1c1c6ddd 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
@@ -23,6 +23,7 @@ class btIDebugDraw;
 #include "btSolverConstraint.h"
 
 
+
 ///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)
@@ -31,12 +32,12 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
 
 	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:
 	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);
 	
@@ -52,18 +53,17 @@ protected:
         const btSolverConstraint& contactConstraint,
         const btContactSolverInfo& solverInfo);
 
-	void	resolveSingleConstraintRow(
-		btSolverBody& body1,
-		btSolverBody& body2,
-		const btSolverConstraint& contactConstraint);
+	//internal method
+	int	getOrInitSolverBody(btCollisionObject& body);
 
-	void	resolveSingleFrictionCacheFriendly(
-		btSolverBody& body1,
-		btSolverBody& body2,
-		const btSolverConstraint& contactConstraint,
-		const btContactSolverInfo& solverInfo,
-		btScalar appliedNormalImpulse);
+	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:
 
 	
diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
index 2abe61e2a..990bd00e9 100755
--- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
@@ -153,27 +153,41 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co
 
 //-----------------------------------------------------------------------------
 
-void btSliderConstraint::solveConstraint(btScalar timeStep)
+void btSliderConstraint::getInfo1 (btConstraintInfo1* info)
+{
+	info->m_numConstraintRows = 0;
+	info->nub = 0;
+}
+
+void btSliderConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	btAssert(0);
+}
+
+
+void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
 {
     m_timeStep = timeStep;
 	if(m_useLinearReferenceFrameA)
 	{
-		solveConstraintInt(m_rbA, m_rbB);
+		solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB);
 	}
 	else
 	{
-		solveConstraintInt(m_rbB, m_rbA);
+		solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
 	}
 } // btSliderConstraint::solveConstraint()
 
 //-----------------------------------------------------------------------------
 
-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++)
     {
@@ -188,8 +202,18 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
 		// calcutate and apply impulse
 		btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
 		btVector3 impulse_vector = normal * normalImpulse;
-		rbA.applyImpulse( impulse_vector, m_relPosA);
-		rbB.applyImpulse(-impulse_vector, m_relPosB);
+		
+		//rbA.applyImpulse( impulse_vector, m_relPosA);
+		//rbB.applyImpulse(-impulse_vector, m_relPosB);
+		{
+			btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
+			btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
+			bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
+			bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
+		}
+
+
+
 		if(m_poweredLinMotor && (!i))
 		{ // apply linear motor
 			if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
@@ -215,8 +239,18 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
 				m_accumulatedLinMotorImpulse = new_acc;
 				// 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);
+				}
+
+
+
 			}
 		}
     }
@@ -225,8 +259,10 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
 	btVector3 axisA =  m_calculatedTransformA.getBasis().getColumn(0);
 	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);
 	btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
@@ -236,24 +272,38 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
 	btVector3 velrelOrthog = angAorthog-angBorthog;
 	//solve orthogonal angular velocity correction
 	btScalar len = velrelOrthog.length();
+	btScalar orthorImpulseMag = 0.f;
+
 	if (len > btScalar(0.00001))
 	{
 		btVector3 normal = velrelOrthog.normalized();
 		btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
-		velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+		//velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+		orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
 	}
 	//solve angular positional correction
 	btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
+	btVector3 angularAxis = angularError;
+	btScalar angularImpulseMag = 0;
+
 	btScalar len2 = angularError.length();
 	if (len2>btScalar(0.00001))
 	{
 		btVector3 normal2 = angularError.normalized();
 		btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
-		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
 	if(m_solveAngLim)
@@ -267,8 +317,14 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
 		impulseMag *= m_kAngle * m_softnessDirAng;
 	}
 	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) 
 	{
@@ -299,8 +355,11 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
 			m_accumulatedAngMotorImpulse = new_acc;
 			// 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);
 		}
 	}
 } // btSliderConstraint::solveConstraint()
diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
index 7f8d40948..94baf61ad 100755
--- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
@@ -126,7 +126,13 @@ public:
     btSliderConstraint();
 	// 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; }
     const btRigidBody& getRigidBodyB() const { return m_rbB; }
@@ -202,7 +208,7 @@ public:
 	btScalar getAngDepth() { return m_angDepth; }
 	// 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);
diff --git a/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/src/BulletDynamics/ConstraintSolver/btSolverBody.h
index 5b812a0d0..cab8a56a5 100644
--- a/src/BulletDynamics/ConstraintSolver/btSolverBody.h
+++ b/src/BulletDynamics/ConstraintSolver/btSolverBody.h
@@ -23,193 +23,157 @@ class	btRigidBody;
 #include "LinearMath/btAlignedAllocator.h"
 #include "LinearMath/btTransformUtil.h"
 
-#ifndef _USE_JACOBIAN
+///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
+#if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
+#define USE_SIMD 1
+#endif //
 
-///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
-ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
+
+#ifdef USE_SIMD
+#include <emmintrin.h>
+
+struct	btSimdScalar
 {
-	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
-	btMatrix3x3		m_worldInvInertiaTensor;
-
-	btVector3		m_angularVelocity;
-	btVector3		m_linearVelocity;
-
-	btVector3		m_deltaLinearVelocity;
-	btVector3		m_deltaAngularVelocity;
-
-	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	btSimdScalar()
 	{
-		velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
+
 	}
 
-	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	SIMD_FORCE_INLINE	btSimdScalar(float	fl)
+	:m_vec128 (_mm_set1_ps(fl))
 	{
-		//if (m_invMass)
-		{
-			m_deltaLinearVelocity += linearComponent*impulseMagnitude;
-			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
-			m_linearVelocity += linearComponent*impulseMagnitude;
-			m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
-		}
+	}
+
+	SIMD_FORCE_INLINE	btSimdScalar(__m128 v128)
+		:m_vec128(v128)
+	{
+	}
+	union
+	{
+		__m128	m_vec128;
+		float	m_floats[4];
+		int		m_ints[4];
+	};
+	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 void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
-	{
-		//if (m_invMass)
-		{
-			m_pushVelocity += linearComponent*impulseMagnitude;
-			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
-		}
+	SIMD_FORCE_INLINE	operator float() const 
+	{ 
+		return m_floats[0]; 
 	}
 
-	
-	void	writebackVelocity()
-	{
-		if (m_invMass)
-		{
-			m_originalBody->setLinearVelocity(m_linearVelocity);
-			m_originalBody->setAngularVelocity(m_angularVelocity);
-			
-			//m_originalBody->setCompanionId(-1);
-		}
-	}
-	
-
-	void	writebackVelocity(btScalar timeStep)
-	{
-		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->setCompanionId(-1);
-		}
-	}
-
-	void	readVelocity()
-	{
-		if (m_invMass)
-		{
-			m_linearVelocity = m_originalBody->getLinearVelocity();
-			m_angularVelocity = m_originalBody->getAngularVelocity();
-		}
-	}
-
-	
-
-
 };
+
+///@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.
 ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
-	
-	btMatrix3x3		m_worldInvInertiaTensor;
-
-	btVector3		m_angularVelocity1;
-	btVector3		m_linearVelocity1;
-
-	btVector3		m_deltaAngularVelocity;
 	btVector3		m_deltaLinearVelocity;
-
-	btVector3		m_centerOfMassPosition;
-	btVector3		m_pushVelocity;
-	btVector3		m_turnVelocity;
-
-	float			m_angularFactor;
-	float			m_invMass;
-	float			m_friction;
+	btVector3		m_deltaAngularVelocity;
+	btScalar		m_angularFactor;
+	btScalar		m_invMass;
+	btScalar		m_friction;
 	btRigidBody*	m_originalBody;
-	
-	
+	btVector3		m_pushVelocity;
+	//btVector3		m_turnVelocity;	
 
-	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_linearVelocity1 + m_deltaLinearVelocity) + (m_angularVelocity1+m_deltaAngularVelocity).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);
 	}
 
-	//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	getAngularVelocity(btVector3& angVel) const
 	{
-		if (m_invMass)
+		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 applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
+	{
+		//if (m_invMass)
 		{
 			m_deltaLinearVelocity += linearComponent*impulseMagnitude;
 			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
 		}
 	}
-	
-	SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
-	{
-		if (m_invMass)
-		{
-			m_pushVelocity += linearComponent*impulseMagnitude;
-			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
-		}
-	}
 
 	
+/*
+	
 	void	writebackVelocity()
 	{
 		if (m_invMass)
 		{
-			m_originalBody->setLinearVelocity(m_linearVelocity1 + m_deltaLinearVelocity);
-			m_originalBody->setAngularVelocity(m_angularVelocity1+m_deltaAngularVelocity);
+			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_linearVelocity1 + m_deltaLinearVelocity);
-			m_originalBody->setAngularVelocity(m_angularVelocity1+m_deltaAngularVelocity);
-			
-			//correct the position/orientation based on push/turn recovery
-			btTransform newTransform;
-			btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
-			m_originalBody->setWorldTransform(newTransform);
-			
+			m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
+			m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
 			//m_originalBody->setCompanionId(-1);
 		}
 	}
-
-	void	readVelocity()
-	{
-		if (m_invMass)
-		{
-			m_linearVelocity1 = m_originalBody->getLinearVelocity();
-			m_angularVelocity1 = m_originalBody->getAngularVelocity();
-		}
-	}
-
 	
 
 
 };
 
-#endif //USE_JAC
 #endif //BT_SOLVER_BODY_H
 
 
diff --git a/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
index 1ebf08df6..459e406db 100644
--- a/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
@@ -21,28 +21,26 @@ class	btRigidBody;
 #include "LinearMath/btMatrix3x3.h"
 #include "btJacobianEntry.h"
 
+#include <emmintrin.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.
 ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-#ifdef _USE_JACOBIAN
-	btJacobianEntry	m_jac;
-#endif
+	btVector3		m_relpos1CrossNormal;
+	btVector3		m_contactNormal;
 
-
-	btVector3	m_relpos1CrossNormal;
-	btVector3	m_contactNormal;
-
-	btVector3	m_relpos2CrossNormal;
-	btVector3	m_angularComponentA;
+	btVector3		m_relpos2CrossNormal;
+	btVector3		m_angularComponentA;
 
 	btVector3	m_angularComponentB;
 	
-	mutable btScalar	m_appliedPushImpulse;
-	mutable btScalar	m_appliedImpulse;
+	mutable btSimdScalar	m_appliedPushImpulse;
+	mutable btSimdScalar	m_appliedImpulse;
 	
 	int			m_solverBodyIdA;
 	int			m_solverBodyIdB;
@@ -51,16 +49,14 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
 	btScalar	m_restitution;
 	btScalar	m_jacDiagABInv;
 	btScalar	m_penetration;
-	
-
-	
+		
 	int			m_constraintType;
 	int			m_frictionIndex;
 	void*		m_originalContactPoint;
-	btScalar	m_rhs;
-	btScalar	m_cfm;
-	btScalar	m_lowerLimit;
-	btScalar	m_upperLimit;
+	btScalar		m_rhs;
+	btScalar		m_cfm;
+	btScalar		m_lowerLimit;
+	btScalar		m_upperLimit;
 
 	enum		btSolverConstraintType
 	{
@@ -69,9 +65,7 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
 	};
 };
 
-
-
-
+typedef btAlignedObjectArray<btSolverConstraint>	btConstraintArray;
 
 
 #endif //BT_SOLVER_CONSTRAINT_H
diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
index c50ec6ec5..d383239cb 100644
--- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
@@ -18,6 +18,8 @@ subject to the following restrictions:
 
 class btRigidBody;
 #include "LinearMath/btScalar.h"
+#include "btSolverConstraint.h"
+struct  btSolverBody;
 
 enum btTypedConstraintType
 {
@@ -55,13 +57,53 @@ public:
 	btTypedConstraint(btTypedConstraintType type);
 	virtual ~btTypedConstraint() {};
 	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
-
 	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;
+
+	
 	const btRigidBody& getRigidBodyA() const
 	{
 		return m_rbA;
@@ -94,7 +136,7 @@ public:
 	{
 		m_userConstraintId = uid;
 	}
-	
+
 	int getUserConstraintId() const
 	{
 		return m_userConstraintId;
diff --git a/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h b/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h
index e853f1488..4e4728ca6 100644
--- a/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h
+++ b/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h
@@ -140,22 +140,22 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverConstraint
 	}					m_flags;
 
 	// Linear parts, used by all constraints
-	btQuadWordStorage	m_relPos1;
-	btQuadWordStorage	m_relPos2;
-	btQuadWordStorage	m_jacdiagABInv;		//Jacobian inverse multiplied by gamma (damping) for each axis
-	btQuadWordStorage	m_linearBias;		//depth*tau/(dt*gamma) along each axis
+	btVector3			m_relPos1;
+	btVector3	m_relPos2;
+	btVector3	m_jacdiagABInv;		//Jacobian inverse multiplied by gamma (damping) for each axis
+	btVector3	m_linearBias;		//depth*tau/(dt*gamma) along each axis
 
 	// Joint-specific parts
 	union
 	{
 		struct 
 		{
-			btQuadWordStorage	m_frameAinW[3];
-			btQuadWordStorage	m_frameBinW[3];
+			btVector3	m_frameAinW[3];
+			btVector3	m_frameBinW[3];
 
 			// For angular
-			btQuadWordStorage	m_angJacdiagABInv;		//1/j 
-			btQuadWordStorage	m_angularBias;			//error/dt, in x/y.		limit error*bias factor / (dt * relaxation factor) in z
+			btVector3	m_angJacdiagABInv;		//1/j 
+			btVector3	m_angularBias;			//error/dt, in x/y.		limit error*bias factor / (dt * relaxation factor) in z
 			
 			// For limit
 			float				m_limitAccumulatedImpulse;
@@ -168,8 +168,8 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverConstraint
 		
 		struct  
 		{
-			btQuadWordStorage	m_swingAxis;
-			btQuadWordStorage	m_twistAxis;
+			btVector3	m_swingAxis;
+			btVector3	m_twistAxis;
 
 			float				m_swingError;
 			float				m_swingJacInv;
diff --git a/src/LinearMath/btQuadWord.h b/src/LinearMath/btQuadWord.h
index b79438399..c657afd2b 100644
--- a/src/LinearMath/btQuadWord.h
+++ b/src/LinearMath/btQuadWord.h
@@ -24,13 +24,13 @@ subject to the following restrictions:
 #include <altivec.h>
 #endif
 
-/**@brief The btQuadWordStorage class is base class for btVector3 and btQuaternion. 
+/**@brief The btQuadWord class is base class for btVector3 and btQuaternion. 
  * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
  */
 #ifndef USE_LIBSPE2
-ATTRIBUTE_ALIGNED16(class) btQuadWordStorage
+ATTRIBUTE_ALIGNED16(class) btQuadWord
 #else
-class btQuadWordStorage
+class btQuadWord
 #endif
 {
 protected:
@@ -45,15 +45,11 @@ public:
 	{
 		return mVec128;
 	}
+protected:
 #else //__CELLOS_LV2__ __SPU__
 	btScalar	m_floats[4];
 #endif //__CELLOS_LV2__ __SPU__
 
-};
-
-/** @brief The btQuadWord is base-class for vectors, points */
-class	btQuadWord : public btQuadWordStorage
-{
 	public:
   
 
@@ -134,11 +130,7 @@ class	btQuadWord : public btQuadWordStorage
 		//	:m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.))
 		{
 		}
-  /**@brief Copy constructor */
-		SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q)
-		{
-			*((btQuadWordStorage*)this) = q;
-		}
+ 
   /**@brief Three argument constructor (zeros w)
    * @param x Value of x
    * @param y Value of y
diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h
index 4f1dc17ae..cc24b1ede 100644
--- a/src/LinearMath/btQuaternion.h
+++ b/src/LinearMath/btQuaternion.h
@@ -19,6 +19,7 @@ subject to the following restrictions:
 
 
 #include "btVector3.h"
+#include "btQuadWord.h"
 
 /**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */
 class btQuaternion : public btQuadWord {
diff --git a/src/LinearMath/btVector3.h b/src/LinearMath/btVector3.h
index 28a0e55a6..10a5af108 100644
--- a/src/LinearMath/btVector3.h
+++ b/src/LinearMath/btVector3.h
@@ -17,47 +17,77 @@ subject to the following restrictions:
 #ifndef SIMD__VECTOR3_H
 #define SIMD__VECTOR3_H
 
-#include "btQuadWord.h"
 
+#include "btScalar.h"
+#include "btScalar.h"
+#include "btMinMax.h"
+#include <emmintrin.h>
 /**@brief btVector3 can be used to represent 3D points and vectors.
  * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
  * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
  */
-class	btVector3 : public btQuadWord {
 
+ATTRIBUTE_ALIGNED16(class) btVector3
+{
 public:
+
+#if defined (__SPU__) && defined (__CELLOS_LV2__)
+	union {
+		vec_float4 mVec128;
+		btScalar	m_floats[4];
+	};
+public:
+	vec_float4	get128() const
+	{
+		return mVec128;
+	}
+public:
+#else //__CELLOS_LV2__ __SPU__
+#ifdef WIN32
+	union {
+		__m128 mVec128;
+		btScalar	m_floats[4];
+	};
+	SIMD_FORCE_INLINE	__m128	get128() const
+	{
+		return mVec128;
+	}
+	SIMD_FORCE_INLINE	void	set128(__m128 v128)
+	{
+		mVec128 = v128;
+	}
+#else
+	btScalar	m_floats[4];
+#endif
+#endif //__CELLOS_LV2__ __SPU__
+
+	public:
+
   /**@brief No initialization constructor */
 	SIMD_FORCE_INLINE btVector3() {}
 
-  /**@brief Constructor from btQuadWordStorage (btVector3 inherits from this so is also valid)
-   * Note: Vector3 derives from btQuadWordStorage*/
-	SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q) 
-		: btQuadWord(q)
-	{
-	}
+ 
 	
   /**@brief Constructor from scalars 
    * @param x X value
    * @param y Y value 
    * @param z Z value 
    */
-	SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) 
-		:btQuadWord(x,y,z,btScalar(0.))
+	SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)
 	{
+		m_floats[0] = x;
+		m_floats[1] = y;
+		m_floats[2] = z;
+		m_floats[3] = btScalar(0.);
 	}
 
-//	SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) 
-//		: btQuadWord(x,y,z,w)
-//	{
-//	}
-
 	
 /**@brief Add a vector to this one 
  * @param The vector to add to this one */
 	SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
 	{
 
-		m_floats[0] += v.x(); m_floats[1] += v.y(); m_floats[2] += v.z();
+		m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2];
 		return *this;
 	}
 
@@ -66,14 +96,14 @@ public:
    * @param The vector to subtract */
 	SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) 
 	{
-		m_floats[0] -= v.x(); m_floats[1] -= v.y(); m_floats[2] -= v.z();
+		m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2];
 		return *this;
 	}
   /**@brief Scale the vector
    * @param s Scale factor */
 	SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
 	{
-		m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s;
+		m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s;
 		return *this;
 	}
 
@@ -89,7 +119,7 @@ public:
    * @param v The other vector in the dot product */
 	SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
 	{
-		return m_floats[0] * v.x() + m_floats[1] * v.y() + m_floats[2] * v.z();
+		return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2];
 	}
 
   /**@brief Return the length of the vector squared */
@@ -148,30 +178,30 @@ public:
 	SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
 	{
 		return btVector3(
-			m_floats[1] * v.z() - m_floats[2] * v.y(),
-			m_floats[2] * v.x() - m_floats[0] * v.z(),
-			m_floats[0] * v.y() - m_floats[1] * v.x());
+			m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1],
+			m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
+			m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
 	}
 
 	SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
 	{
-		return m_floats[0] * (v1.y() * v2.z() - v1.z() * v2.y()) + 
-			m_floats[1] * (v1.z() * v2.x() - v1.x() * v2.z()) + 
-			m_floats[2] * (v1.x() * v2.y() - v1.y() * v2.x());
+		return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + 
+			m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + 
+			m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
 	}
 
   /**@brief Return the axis with the smallest value 
    * Note return values are 0,1,2 for x, y, or z */
 	SIMD_FORCE_INLINE int minAxis() const
 	{
-		return m_floats[0] < m_floats[1] ? (m_floats[0] < m_floats[2] ? 0 : 2) : (m_floats[1] < m_floats[2] ? 1 : 2);
+		return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
 	}
 
   /**@brief Return the axis with the largest value 
    * Note return values are 0,1,2 for x, y, or z */
 	SIMD_FORCE_INLINE int maxAxis() const 
 	{
-		return m_floats[0] < m_floats[1] ? (m_floats[1] < m_floats[2] ? 2 : 1) : (m_floats[0] < m_floats[2] ? 2 : 0);
+		return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
 	}
 
 	SIMD_FORCE_INLINE int furthestAxis() const
@@ -187,9 +217,9 @@ public:
 	SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
 	{
 		btScalar s = btScalar(1.0) - rt;
-		m_floats[0] = s * v0.x() + rt * v1.x();
-		m_floats[1] = s * v0.y() + rt * v1.y();
-		m_floats[2] = s * v0.z() + rt * v1.z();
+		m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
+		m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
+		m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
 		//don't do the unused w component
 		//		m_co[3] = s * v0[3] + rt * v1[3];
 	}
@@ -199,20 +229,86 @@ public:
    * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
 	SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const 
 	{
-		return btVector3(m_floats[0] + (v.x() - m_floats[0]) * t,
-			m_floats[1] + (v.y() - m_floats[1]) * t,
-			m_floats[2] + (v.z() - m_floats[2]) * t);
+		return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
+			m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
+			m_floats[2] + (v.m_floats[2] -m_floats[2]) * t);
 	}
 
   /**@brief Elementwise multiply this vector by the other 
    * @param v The other vector */
 	SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
 	{
-		m_floats[0] *= v.x(); m_floats[1] *= v.y(); m_floats[2] *= v.z();
+		m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2];
 		return *this;
 	}
 
-	
+	 /**@brief Return the x value */
+		SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
+  /**@brief Return the y value */
+		SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
+  /**@brief Return the z value */
+		SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
+  /**@brief Set the x value */
+		SIMD_FORCE_INLINE void	setX(btScalar x) { m_floats[0] = x;};
+  /**@brief Set the y value */
+		SIMD_FORCE_INLINE void	setY(btScalar y) { m_floats[1] = y;};
+  /**@brief Set the z value */
+		SIMD_FORCE_INLINE void	setZ(btScalar z) {m_floats[2] = z;};
+  /**@brief Set the w value */
+		SIMD_FORCE_INLINE void	setW(btScalar w) { m_floats[3] = w;};
+  /**@brief Return the x value */
+		SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
+  /**@brief Return the y value */
+		SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
+  /**@brief Return the z value */
+		SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
+  /**@brief Return the w value */
+		SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
+
+	//SIMD_FORCE_INLINE btScalar&       operator[](int i)       { return (&m_floats[0])[i];	}      
+	//SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
+	///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
+	SIMD_FORCE_INLINE	operator       btScalar *()       { return &m_floats[0]; }
+	SIMD_FORCE_INLINE	operator const btScalar *() const { return &m_floats[0]; }
+
+	SIMD_FORCE_INLINE	bool	operator==(const btVector3& other) const
+	{
+		return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0]));
+	}
+
+	SIMD_FORCE_INLINE	bool	operator!=(const btVector3& other) const
+	{
+		return !(*this == other);
+	}
+
+	 /**@brief Set each element to the max of the current values and the values of another btVector3
+   * @param other The other btVector3 to compare with 
+   */
+		SIMD_FORCE_INLINE void	setMax(const btVector3& other)
+		{
+			btSetMax(m_floats[0], other.m_floats[0]);
+			btSetMax(m_floats[1], other.m_floats[1]);
+			btSetMax(m_floats[2], other.m_floats[2]);
+			btSetMax(m_floats[3], other.w());
+		}
+  /**@brief Set each element to the min of the current values and the values of another btVector3
+   * @param other The other btVector3 to compare with 
+   */
+		SIMD_FORCE_INLINE void	setMin(const btVector3& other)
+		{
+			btSetMin(m_floats[0], other.m_floats[0]);
+			btSetMin(m_floats[1], other.m_floats[1]);
+			btSetMin(m_floats[2], other.m_floats[2]);
+			btSetMin(m_floats[3], other.w());
+		}
+
+		SIMD_FORCE_INLINE void 	setValue(const btScalar& x, const btScalar& y, const btScalar& z)
+		{
+			m_floats[0]=x;
+			m_floats[1]=y;
+			m_floats[2]=z;
+			m_floats[3] = 0.f;
+		}
 
 };
 
@@ -220,34 +316,34 @@ public:
 SIMD_FORCE_INLINE btVector3 
 operator+(const btVector3& v1, const btVector3& v2) 
 {
-	return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z());
+	return btVector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]);
 }
 
 /**@brief Return the elementwise product of two vectors */
 SIMD_FORCE_INLINE btVector3 
 operator*(const btVector3& v1, const btVector3& v2) 
 {
-	return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z());
+	return btVector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]);
 }
 
 /**@brief Return the difference between two vectors */
 SIMD_FORCE_INLINE btVector3 
 operator-(const btVector3& v1, const btVector3& v2)
 {
-	return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z());
+	return btVector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]);
 }
 /**@brief Return the negative of the vector */
 SIMD_FORCE_INLINE btVector3 
 operator-(const btVector3& v)
 {
-	return btVector3(-v.x(), -v.y(), -v.z());
+	return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
 }
 
 /**@brief Return the vector scaled by s */
 SIMD_FORCE_INLINE btVector3 
 operator*(const btVector3& v, const btScalar& s)
 {
-	return btVector3(v.x() * s, v.y() * s, v.z() * s);
+	return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
 }
 
 /**@brief Return the vector scaled by s */
@@ -269,7 +365,7 @@ operator/(const btVector3& v, const btScalar& s)
 SIMD_FORCE_INLINE btVector3
 operator/(const btVector3& v1, const btVector3& v2)
 {
-	return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z());
+	return btVector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]);
 }
 
 /**@brief Return the dot product between two vectors */
@@ -325,11 +421,7 @@ lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
 	return v1.lerp(v2, t);
 }
 
-/**@brief Test if each element of the vector is equivalent */
-SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2) 
-{
-	return p1.x() == p2.x() && p1.y() == p2.y() && p1.z() == p2.z();
-}
+
 
 SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
 {
@@ -404,7 +496,7 @@ public:
 		if (m_floats[2] > maxVal)
 		{
 			maxIndex = 2;
-			maxVal = m_floats[2];
+			maxVal =m_floats[2];
 		}
 		if (m_floats[3] > maxVal)
 		{
@@ -437,7 +529,7 @@ public:
 		if (m_floats[2] < minVal)
 		{
 			minIndex = 2;
-			minVal = m_floats[2];
+			minVal =m_floats[2];
 		}
 		if (m_floats[3] < minVal)
 		{
@@ -455,6 +547,40 @@ public:
 		return absolute4().maxAxis4();
 	}
 
+	
+ 
+
+  /**@brief Set x,y,z and zero w 
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   */
+		
+
+/*		void getValue(btScalar *m) const 
+		{
+			m[0] = m_floats[0];
+			m[1] = m_floats[1];
+			m[2] =m_floats[2];
+		}
+*/
+/**@brief Set the values 
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   * @param w Value of w
+   */
+		SIMD_FORCE_INLINE void	setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
+		{
+			m_floats[0]=x;
+			m_floats[1]=y;
+			m_floats[2]=z;
+			m_floats[3]=w;
+		}
+
+
+ 
+
 };