Added getInfo1()/ getInfo2() code for joints to support the new SIMD constraint solver

src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

@@ -24,7 +24,8 @@ Written by: Marcus Hennix
 
 btConeTwistConstraint::btConeTwistConstraint()
 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
-m_useSolveConstraintObsolete(true)
+m_useSolveConstraintObsolete(false)
+//m_useSolveConstraintObsolete(true)
 {
 }
 
@@ -33,7 +34,8 @@ 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_useSolveConstraintObsolete(true)
+											 m_useSolveConstraintObsolete(false)
+//											 m_useSolveConstraintObsolete(true)
 {
 	m_swingSpan1 = btScalar(1e30);
 	m_swingSpan2 = btScalar(1e30);
@@ -49,7 +51,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_useSolveConstraintObsolete(true)
+											 m_useSolveConstraintObsolete(false)
+//											 m_useSolveConstraintObsolete(true)
 {
 	m_rbBFrame = m_rbAFrame;
 	
@@ -64,6 +67,7 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform&
 	
 }			
 
+//-----------------------------------------------------------------------------
 
 void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
 {
@@ -71,17 +75,124 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
 	{
 		info->m_numConstraintRows = 0;
 		info->nub = 0;
-	} else
+	} 
+	else
 	{
-		btAssert(0);
+		info->m_numConstraintRows = 3;
+		info->nub = 3;
+		calcAngleInfo();
+		if(m_solveSwingLimit)
+		{
+			info->m_numConstraintRows++;
+			info->nub--;
+		}
+		if(m_solveTwistLimit)
+		{
+			info->m_numConstraintRows++;
+			info->nub--;
+		}
+	}
+} // btConeTwistConstraint::getInfo1()
+	
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	btAssert(!m_useSolveConstraintObsolete);
+	//retrieve matrices
+	btTransform body0_trans;
+	body0_trans = m_rbA.getCenterOfMassTransform();
+    btTransform body1_trans;
+	body1_trans = m_rbB.getCenterOfMassTransform();
+    // set jacobian
+    info->m_J1linearAxis[0] = 1;
+    info->m_J1linearAxis[info->rowskip+1] = 1;
+    info->m_J1linearAxis[2*info->rowskip+2] = 1;
+	btVector3 a1 = body0_trans.getBasis() * m_rbAFrame.getOrigin();
+	{
+		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+		btVector3 a1neg = -a1;
+		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+	btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin();
+	{
+		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
+		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+    // set right hand side
+    btScalar k = info->fps * info->erp;
+    int j;
+	for (j=0; j<3; j++)
+    {
+        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
+		info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
+		info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
+    }
+	int row = 3;
+    int srow = row * info->rowskip;
+	btVector3 ax1;
+	// angular limits
+	if(m_solveSwingLimit)
+	{
+		ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
+        btScalar *J1 = info->m_J1angularAxis;
+        btScalar *J2 = info->m_J2angularAxis;
+        J1[srow+0] = ax1[0];
+        J1[srow+1] = ax1[1];
+        J1[srow+2] = ax1[2];
+        J2[srow+0] = -ax1[0];
+        J2[srow+1] = -ax1[1];
+        J2[srow+2] = -ax1[2];
+		btScalar k = info->fps * m_biasFactor;
+		info->m_constraintError[srow] = k * m_swingCorrection;
+		info->cfm[srow] = 0.0f;
+		// m_swingCorrection is always positive or 0
+		info->m_lowerLimit[srow] = 0;
+		info->m_upperLimit[srow] = SIMD_INFINITY;
+		srow += info->rowskip;
+	}
+	if(m_solveTwistLimit)
+	{
+		ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
+        btScalar *J1 = info->m_J1angularAxis;
+        btScalar *J2 = info->m_J2angularAxis;
+        J1[srow+0] = ax1[0];
+        J1[srow+1] = ax1[1];
+        J1[srow+2] = ax1[2];
+        J2[srow+0] = -ax1[0];
+        J2[srow+1] = -ax1[1];
+        J2[srow+2] = -ax1[2];
+		btScalar k = info->fps * m_biasFactor;
+		info->m_constraintError[srow] = k * m_twistCorrection;
+		info->cfm[srow] = 0.0f;
+		if(m_twistSpan > 0.0f)
+		{
+
+			if(m_twistCorrection > 0.0f)
+			{
+				info->m_lowerLimit[srow] = 0;
+				info->m_upperLimit[srow] = SIMD_INFINITY;
+			} 
+			else
+			{
+				info->m_lowerLimit[srow] = -SIMD_INFINITY;
+				info->m_upperLimit[srow] = 0;
+			} 
+		}
+		else
+		{
+			info->m_lowerLimit[srow] = -SIMD_INFINITY;
+			info->m_upperLimit[srow] = SIMD_INFINITY;
+		}
+		srow += info->rowskip;
 	}
 }
 	
-void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
+//-----------------------------------------------------------------------------
-{
-	btAssert(0);
-}
-	
 
 void	btConeTwistConstraint::buildJacobian()
 {
@@ -227,6 +338,8 @@ void	btConeTwistConstraint::buildJacobian()
 	}
 }
 
+//-----------------------------------------------------------------------------
+
 void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
 {
 	if (m_useSolveConstraintObsolete)
@@ -315,8 +428,101 @@ void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
 
 }
 
+//-----------------------------------------------------------------------------
+
 void	btConeTwistConstraint::updateRHS(btScalar	timeStep)
 {
 	(void)timeStep;
 
 }
+
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::calcAngleInfo()
+{
+	m_swingCorrection = btScalar(0.);
+	m_twistLimitSign = btScalar(0.);
+	m_solveTwistLimit = false;
+	m_solveSwingLimit = false;
+
+	btVector3 b1Axis1,b1Axis2,b1Axis3;
+	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);
+		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);			
+		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;
+	}
+
+	// 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.);
+		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
+		if (twist <= -m_twistSpan*lockedFreeFactor)
+		{
+			m_twistCorrection = -(twist + m_twistSpan);
+			m_solveTwistLimit = true;
+			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
+			m_twistAxis.normalize();
+			m_twistAxis *= -1.0f;
+		}
+		else if (twist >  m_twistSpan*lockedFreeFactor)
+		{
+			m_twistCorrection = (twist - m_twistSpan);
+			m_solveTwistLimit = true;
+			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
+			m_twistAxis.normalize();
+		}
+	}
+} // btConeTwistConstraint::calcAngleInfo()
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------

src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

@@ -127,6 +127,9 @@ public:
 		return m_twistLimitSign;
 	}
 
+	void calcAngleInfo();
+
+
 };
 
 #endif //CONETWISTCONSTRAINT_H

src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

@@ -25,27 +25,35 @@ http://gimpact.sf.net
 #include "LinearMath/btTransformUtil.h"
 #include <new>
 
+//-----------------------------------------------------------------------------
 
 btGeneric6DofConstraint::btGeneric6DofConstraint()
 :btTypedConstraint(D6_CONSTRAINT_TYPE),
 m_useLinearReferenceFrameA(true),
-m_useSolveConstraintObsolete(true)
+m_useSolveConstraintObsolete(false)
+//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)
+m_useSolveConstraintObsolete(false)
+//m_useSolveConstraintObsolete(true)
 {
 
 }
+//-----------------------------------------------------------------------------
 
 
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
 
+//-----------------------------------------------------------------------------
+
 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
 {
@@ -54,6 +62,8 @@ btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
 	return mat[i][j];
 }
 
+//-----------------------------------------------------------------------------
+
 ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
 bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
 bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
@@ -63,9 +73,10 @@ bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
 	//	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
 	//
 
-	if (btGetMatrixElem(mat,2) < btScalar(1.0))
+	btScalar fi = btGetMatrixElem(mat,2);
+	if (fi < btScalar(1.0f))
 	{
-		if (btGetMatrixElem(mat,2) > btScalar(-1.0))
+		if (fi > btScalar(-1.0f))
 		{
 			xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
 			xyz[1] = btAsin(btGetMatrixElem(mat,2));
@@ -87,18 +98,12 @@ bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
 		xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
 		xyz[1] = SIMD_HALF_PI;
 		xyz[2] = 0.0;
-
 	}
-
-
 	return false;
 }
 
-
-
 //////////////////////////// btRotationalLimitMotor ////////////////////////////////////
 
-
 int btRotationalLimitMotor::testLimitValue(btScalar test_value)
 {
 	if(m_loLimit>m_hiLimit)
@@ -125,6 +130,7 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value)
 
 }
 
+//-----------------------------------------------------------------------------
 
 btScalar btRotationalLimitMotor::solveAngularLimits(
 	btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
@@ -211,7 +217,43 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
 
 //////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
 
+
+
+
 //////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
+
+
+int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
+{
+	btScalar loLimit = m_lowerLimit[limitIndex];
+	btScalar hiLimit = m_upperLimit[limitIndex];
+	if(loLimit > hiLimit)
+	{
+		m_currentLimit[limitIndex] = 0;//Free from violation
+		m_currentLimitError[limitIndex] = btScalar(0.f);
+		return 0;
+	}
+
+	if (test_value < loLimit)
+	{
+		m_currentLimit[limitIndex] = 2;//low limit violation
+		m_currentLimitError[limitIndex] =  test_value - loLimit;
+		return 2;
+	}
+	else if (test_value> hiLimit)
+	{
+		m_currentLimit[limitIndex] = 1;//High limit violation
+		m_currentLimitError[limitIndex] = test_value - hiLimit;
+		return 1;
+	};
+
+	m_currentLimit[limitIndex] = 0;//Free from violation
+	m_currentLimitError[limitIndex] = btScalar(0.f);
+	return 0;
+} // btTranslationalLimitMotor::testLimitValue()
+
+//-----------------------------------------------------------------------------
+
 btScalar btTranslationalLimitMotor::solveLinearAxis(
 	btScalar timeStep,
 	btScalar jacDiagABInv,
@@ -300,19 +342,10 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
 
 //////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
 
-
-
-
-
-
 void btGeneric6DofConstraint::calculateAngleInfo()
 {
 	btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
-
 	matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
-
-
-
 	// in euler angle mode we do not actually constrain the angular velocity
 	// along the axes axis[0] and axis[2] (although we do use axis[1]) :
 	//
@@ -327,7 +360,6 @@ void btGeneric6DofConstraint::calculateAngleInfo()
 	// 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);
 
@@ -335,28 +367,23 @@ void btGeneric6DofConstraint::calculateAngleInfo()
 	m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
 	m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
 
-
+	m_calculatedAxis[0].normalize();
-	//    if(m_debugDrawer)
+	m_calculatedAxis[1].normalize();
-	//    {
+	m_calculatedAxis[2].normalize();
-	//
-	//    	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;
-
+	calculateLinearInfo();
 	calculateAngleInfo();
 }
 
+//-----------------------------------------------------------------------------
 
 void btGeneric6DofConstraint::buildLinearJacobian(
 	btJacobianEntry & jacLinear,const btVector3 & normalWorld,
@@ -374,6 +401,8 @@ void btGeneric6DofConstraint::buildLinearJacobian(
         m_rbB.getInvMass());
 }
 
+//-----------------------------------------------------------------------------
+
 void btGeneric6DofConstraint::buildAngularJacobian(
 	btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
 {
@@ -385,15 +414,18 @@ void btGeneric6DofConstraint::buildAngularJacobian(
 
 }
 
+//-----------------------------------------------------------------------------
+
 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
 {
 	btScalar angle = m_calculatedAxisAngleDiff[axis_index];
-
 	//test limits
 	m_angularLimits[axis_index].testLimitValue(angle);
 	return m_angularLimits[axis_index].needApplyTorques();
 }
 
+//-----------------------------------------------------------------------------
+
 void btGeneric6DofConstraint::buildJacobian()
 {
 	if (m_useSolveConstraintObsolete)
@@ -452,6 +484,8 @@ void btGeneric6DofConstraint::buildJacobian()
 	}
 }
 
+//-----------------------------------------------------------------------------
+
 void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
 {
 	if (m_useSolveConstraintObsolete)
@@ -462,260 +496,80 @@ void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
 	{
 		//prepare constraint
 		calculateTransforms();
-		info->m_numConstraintRows = 3;
+		info->m_numConstraintRows = 0;
-		info->nub = 3;//??
+		info->nub = 6;
+		int i;
+		//test linear limits
+		for(i = 0; i < 3; i++)
+		{
+			if(m_linearLimits.needApplyForce(i))
+			{
+				info->m_numConstraintRows++;
+				info->nub--;
+			}
+		}
 		//test angular limits
 		for (int i=0;i<3 ;i++ )
 		{
-			//if(i==2) continue;
 			if(testAngularLimitMotor(i))
 			{
 				info->m_numConstraintRows++;
+				info->nub--;
 			}
 		}
 	}
 }
 
+//-----------------------------------------------------------------------------
+
 void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
 {
 	btAssert(!m_useSolveConstraintObsolete);
-
 	int row = setLinearLimits(info);
 	setAngularLimits(info, row);
 }
 
+//-----------------------------------------------------------------------------
+
 int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
 {
-
 	btGeneric6DofConstraint * d6constraint = this;
-
+	int row = 0;
-	//retrieve matrices
+	//solve linear limits
-	btTransform body0_trans;
+	btRotationalLimitMotor limot;
-	body0_trans = m_rbA.getCenterOfMassTransform();
+	bool useBasisBodyB = (m_rbB.getInvMass() == btScalar(0.f));
-
+	for (int i=0;i<3 ;i++ )
-	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;
-
-	/*for (int i=0;i<3;i++)
 	{
-		if (m_useLinearReferenceFrameA)
+		if(m_linearLimits.needApplyForce(i))
-		{
+		{ // re-use rotational motor code
-			btVector3* linear_axis = (btVector3* )&info->m_J1linearAxis[s*i];
+			limot.m_bounce = btScalar(0.f);
-			*linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
+			limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
-		}
+			limot.m_currentLimitError  = -m_linearLimits.m_currentLimitError[i];
-		else
+			limot.m_damping  = m_linearLimits.m_damping;
-		{
+			limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
-			btVector3* linear_axis = (btVector3* )&info->m_J1linearAxis[s*i];
+			limot.m_ERP  = m_linearLimits.m_restitution;
-			*linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
+			limot.m_hiLimit  = m_linearLimits.m_upperLimit[i];
+			limot.m_limitSoftness  = m_linearLimits.m_limitSoftness;
+			limot.m_loLimit  = m_linearLimits.m_lowerLimit[i];
+			limot.m_maxLimitForce  = btScalar(0.f);
+			limot.m_maxMotorForce  = m_linearLimits.m_maxMotorForce[i];
+			limot.m_targetVelocity  = m_linearLimits.m_targetVelocity[i];
+			btVector3 axis;
+			if(useBasisBodyB)
+			{
+				axis = m_calculatedTransformB.getBasis().getColumn(i);
+			}
+			else
+			{
+				axis = m_calculatedTransformA.getBasis().getColumn(i);
+			}
+			row += get_limit_motor_info2(&limot,	&m_rbA, &m_rbB, info, row, axis, 0);
 		}
 	}
-	*/
+	return row;
-
-
-
-	btVector3 a1,a2;
-
-	a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin();
-
-	{
-		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
-		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
-		btVector3 a1neg = -a1;
-		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
-	}
-
-	/*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();
-
-	{
-		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
-		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
-		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
-		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
-	}
-
-	// set right hand side
-	btScalar k = info->fps * info->erp;
-	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;
-
 }
 
-
+//-----------------------------------------------------------------------------
-/*! \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)
-{
-
-
-    int srow = row * info->rowskip;
-
-    // if the joint is powered, or has joint limits, add in the extra row
-    int powered = limot->m_enableMotor;
-    int limit = limot->m_currentLimit;
-
-    if (powered || limit)
-    {
-        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)
-        {
-            J2[srow+0] = -ax1[0];
-            J2[srow+1] = -ax1[1];
-            J2[srow+2] = -ax1[2];
-        }
-
-        // 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]);
-
-			ltd = c.cross(ax1);
-
-            info->m_J1angularAxis[srow+0] = ltd[0];
-            info->m_J1angularAxis[srow+1] = ltd[1];
-            info->m_J1angularAxis[srow+2] = ltd[2];
-            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)
 {
@@ -728,7 +582,7 @@ int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_o
 		if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
 		{
 			btVector3 axis = d6constraint->getAxis(i);
-			row += bt_get_limit_motor_info2(
+			row += get_limit_motor_info2(
 				d6constraint->getRotationalLimitMotor(i),
 				&m_rbA,
 				&m_rbB,
@@ -739,8 +593,7 @@ int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_o
 	return row;
 }
 
-///////////////////limit motor support
+//-----------------------------------------------------------------------------
-
 
 void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
 {
@@ -801,22 +654,30 @@ void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolv
 	}
 }
 
+//-----------------------------------------------------------------------------
+
 void	btGeneric6DofConstraint::updateRHS(btScalar	timeStep)
 {
 	(void)timeStep;
 
 }
 
+//-----------------------------------------------------------------------------
+
 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
 {
 	return m_calculatedAxis[axis_index];
 }
 
+//-----------------------------------------------------------------------------
+
 btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
 {
 	return m_calculatedAxisAngleDiff[axis_index];
 }
 
+//-----------------------------------------------------------------------------
+
 void btGeneric6DofConstraint::calcAnchorPos(void)
 {
 	btScalar imA = m_rbA.getInvMass();
@@ -836,3 +697,160 @@ void btGeneric6DofConstraint::calcAnchorPos(void)
 	return;
 } // btGeneric6DofConstraint::calcAnchorPos()
 
+//-----------------------------------------------------------------------------
+
+void btGeneric6DofConstraint::calculateLinearInfo()
+{
+	m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
+	m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
+	for(int i = 0; i < 3; i++)
+	{
+		m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
+	}
+} // btGeneric6DofConstraint::calculateLinearInfo()
+
+//-----------------------------------------------------------------------------
+
+int btGeneric6DofConstraint::get_limit_motor_info2(
+	btRotationalLimitMotor * limot,
+	btRigidBody * body0, btRigidBody * body1,
+	btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
+{
+    int srow = row * info->rowskip;
+    int powered = limot->m_enableMotor;
+    int limit = limot->m_currentLimit;
+    if (powered || limit)
+    {   // if the joint is powered, or has joint limits, add in the extra row
+        btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
+        btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
+        J1[srow+0] = ax1[0];
+        J1[srow+1] = ax1[1];
+        J1[srow+2] = ax1[2];
+        if(rotational)
+        {
+            J2[srow+0] = -ax1[0];
+            J2[srow+1] = -ax1[1];
+            J2[srow+2] = -ax1[2];
+        }
+        // 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
+        // at center of mass of two bodies. 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)
+        {
+			btVector3 c = body1->getCenterOfMassPosition() - body0->getCenterOfMassPosition();
+			ltd = c.cross(ax1);
+			btScalar miA = body0->getInvMass();
+			btScalar miB = body1->getInvMass();
+			btScalar miS = miA + miB;
+			btScalar factA, factB;
+			if(miS > btScalar(0.f))
+			{
+				factA = miB / miS;
+			}
+			else 
+			{
+				factA = btScalar(0.5f);
+			}
+			if(factA > 0.99f) factA = 0.99f;
+			if(factA < 0.01f) factA = 0.01f;
+			factB = btScalar(1.0f) - factA;
+            info->m_J1angularAxis[srow+0] = factA * ltd[0];
+            info->m_J1angularAxis[srow+1] = factA * ltd[1];
+            info->m_J1angularAxis[srow+2] = factA * ltd[2];
+            info->m_J2angularAxis[srow+0] = factB * ltd[0];
+            info->m_J2angularAxis[srow+1] = factB * ltd[1];
+            info->m_J2angularAxis[srow+2] = factB * ltd[2];
+        }
+        // if 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;
+            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;
+            if (limot->m_loLimit == limot->m_hiLimit)
+            {   // limited low and high simultaneously
+                info->m_lowerLimit[srow] = -SIMD_INFINITY;
+                info->m_upperLimit[srow] = SIMD_INFINITY;
+            }
+            else
+            {
+                if (limit == 1)
+                {
+                    info->m_lowerLimit[srow] = 0;
+                    info->m_upperLimit[srow] = SIMD_INFINITY;
+                }
+                else
+                {
+                    info->m_lowerLimit[srow] = -SIMD_INFINITY;
+                    info->m_upperLimit[srow] = 0;
+                }
+                // deal with bounce
+                if (limot->m_bounce > 0)
+                {
+                    // calculate joint velocity
+                    btScalar vel;
+                    if (rotational)
+                    {
+                        vel = body0->getAngularVelocity().dot(ax1);
+                        if (body1)
+                            vel -= body1->getAngularVelocity().dot(ax1);
+                    }
+                    else
+                    {
+                        vel = body0->getLinearVelocity().dot(ax1);
+                        if (body1)
+                            vel -= body1->getLinearVelocity().dot(ax1);
+                    }
+                    // only apply bounce if the velocity is incoming, and if the
+                    // resulting c[] exceeds what we already have.
+                    if (limit == 1)
+                    {
+                        if (vel < 0)
+                        {
+                            btScalar newc = -limot->m_bounce* vel;
+                            if (newc > info->m_constraintError[srow]) 
+								info->m_constraintError[srow] = newc;
+                        }
+                    }
+                    else
+                    {
+                        if (vel > 0)
+                        {
+                            btScalar newc = -limot->m_bounce * vel;
+                            if (newc < info->m_constraintError[srow]) 
+								info->m_constraintError[srow] = newc;
+                        }
+                    }
+                }
+            }
+        }
+        return 1;
+    }
+    else return 0;
+}
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------

src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

@@ -30,6 +30,8 @@ http://gimpact.sf.net
 class btRigidBody;
 
 
+
+
 //! Rotation Limit structure for generic joints
 class btRotationalLimitMotor
 {
@@ -92,7 +94,7 @@ public:
 	//! Is limited
     bool isLimited()
     {
-    	if(m_loLimit>=m_hiLimit) return false;
+    	if(m_loLimit > m_hiLimit) return false;
     	return true;
     }
 
@@ -112,7 +114,6 @@ public:
 	//! apply the correction impulses for two bodies
     btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
 
-
 };
 
 
@@ -129,6 +130,11 @@ public:
     btScalar	m_damping;//!< Damping for linear limit
     btScalar	m_restitution;//! Bounce parameter for linear limit
     //!@}
+	bool		m_enableMotor[3];
+    btVector3	m_targetVelocity;//!< target motor velocity
+    btVector3	m_maxMotorForce;//!< max force on motor
+    btVector3	m_currentLimitError;//!  How much is violated this limit
+    int			m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
 
     btTranslationalLimitMotor()
     {
@@ -139,6 +145,12 @@ public:
     	m_limitSoftness = 0.7f;
     	m_damping = btScalar(1.0f);
     	m_restitution = btScalar(0.5f);
+		for(int i=0; i < 3; i++) 
+		{
+			m_enableMotor[i] = false;
+			m_targetVelocity[i] = btScalar(0.f);
+			m_maxMotorForce[i] = btScalar(0.f);
+		}
     }
 
     btTranslationalLimitMotor(const btTranslationalLimitMotor & other )
@@ -150,6 +162,12 @@ public:
     	m_limitSoftness = other.m_limitSoftness ;
     	m_damping = other.m_damping;
     	m_restitution = other.m_restitution;
+		for(int i=0; i < 3; i++) 
+		{
+			m_enableMotor[i] = other.m_enableMotor[i];
+			m_targetVelocity[i] = other.m_targetVelocity[i];
+			m_maxMotorForce[i] = other.m_maxMotorForce[i];
+		}
     }
 
     //! Test limit
@@ -163,6 +181,12 @@ public:
     {
        return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
     }
+    inline bool needApplyForce(int limitIndex)
+    {
+    	if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
+    	return true;
+    }
+	int testLimitValue(int limitIndex, btScalar test_value);
 
 
     btScalar solveLinearAxis(
@@ -247,6 +271,7 @@ protected:
     btTransform m_calculatedTransformB;
     btVector3 m_calculatedAxisAngleDiff;
     btVector3 m_calculatedAxis[3];
+    btVector3 m_calculatedLinearDiff;
     
 	btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
 
@@ -272,6 +297,8 @@ protected:
 
     void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
 
+	// tests linear limits
+	void calculateLinearInfo();
 
 	//! calcs the euler angles between the two bodies.
     void calculateAngleInfo();
@@ -443,6 +470,11 @@ public:
 
 	virtual void calcAnchorPos(void); // overridable
 
+	int get_limit_motor_info2(	btRotationalLimitMotor * limot,
+								btRigidBody * body0, btRigidBody * body1,
+								btConstraintInfo2 *info, int row, btVector3& ax1, int rotational);
+
+
 };
 
 #endif //GENERIC_6DOF_CONSTRAINT_H

src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

@@ -21,21 +21,26 @@ subject to the following restrictions:
 #include <new>
 #include "btSolverBody.h"
 
+//-----------------------------------------------------------------------------
 
 
 btHingeConstraint::btHingeConstraint()
 : btTypedConstraint (HINGE_CONSTRAINT_TYPE),
 m_enableAngularMotor(false),
-m_useSolveConstraintObsolete(true)
+m_useSolveConstraintObsolete(false)
+//m_useSolveConstraintObsolete(true)
 {
 }
 
+//-----------------------------------------------------------------------------
+
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
 									 btVector3& axisInA,btVector3& axisInB)
 									 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
 									 m_angularOnly(false),
 									 m_enableAngularMotor(false),
-									 m_useSolveConstraintObsolete(true)
+									 m_useSolveConstraintObsolete(false)
+//									 m_useSolveConstraintObsolete(true)
 {
 	m_rbAFrame.getOrigin() = pivotInA;
 	
@@ -78,10 +83,12 @@ 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), m_useSolveConstraintObsolete(true)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), 
+m_useSolveConstraintObsolete(false)
+// m_useSolveConstraintObsolete(true)
 {
 
 	// since no frame is given, assume this to be zero angle and just pick rb transform axis
@@ -115,12 +122,15 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,
 	m_solveLimit = false;
 }
 
+//-----------------------------------------------------------------------------
+
 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_useSolveConstraintObsolete(true)
+m_useSolveConstraintObsolete(false)
+//m_useSolveConstraintObsolete(true)
 {
 	// flip axis
 	m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
@@ -136,13 +146,14 @@ m_useSolveConstraintObsolete(true)
 	m_solveLimit = 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_useSolveConstraintObsolete(true)
+m_useSolveConstraintObsolete(false)
+//m_useSolveConstraintObsolete(true)
 {
 	///not providing rigidbody B means implicitly using worldspace for body B
 
@@ -162,6 +173,8 @@ m_useSolveConstraintObsolete(true)
 	m_solveLimit = false;
 }
 
+//-----------------------------------------------------------------------------
+
 void	btHingeConstraint::buildJacobian()
 {
 	if (m_useSolveConstraintObsolete)
@@ -233,34 +246,11 @@ void	btHingeConstraint::buildJacobian()
 			m_rbA.getInvInertiaDiagLocal(),
 			m_rbB.getInvInertiaDiagLocal());
 
+			// clear accumulator
+			m_accLimitImpulse = btScalar(0.);
 
-		// Compute limit information
+			// test angular limit
-		btScalar hingeAngle = getHingeAngle();  
+			testLimit();
-
-		//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)
-			{
-				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);
@@ -270,24 +260,215 @@ void	btHingeConstraint::buildJacobian()
 	}
 }
 
+//-----------------------------------------------------------------------------
 
-void btHingeConstraint::getInfo1 (btConstraintInfo1* info)
+void btHingeConstraint::getInfo1(btConstraintInfo1* info)
 {
 	if (m_useSolveConstraintObsolete)
 	{
 		info->m_numConstraintRows = 0;
 		info->nub = 0;
-	} else
-	{
-		btAssert(0);
 	}
-}
+	else
+	{
+		info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
+		info->nub = 1; 
+		//prepare constraint
+		testLimit();
+		if(getSolveLimit() || getEnableAngularMotor())
+		{
+			info->m_numConstraintRows++; // limit 3rd anguar as well
+			info->nub--; 
+		}
+	}
+} // btHingeConstraint::getInfo1 ()
+
+//-----------------------------------------------------------------------------
 
 void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
 {
-	btAssert(0);
+	btAssert(!m_useSolveConstraintObsolete);
+	int i, s = info->rowskip;
+	// transforms in world space
+	btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
+	btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
+	// pivot point
+	btVector3 pivotAInW = trA.getOrigin();
+	btVector3 pivotBInW = trB.getOrigin();
+	// linear (all fixed)
+    info->m_J1linearAxis[0] = 1;
+    info->m_J1linearAxis[s + 1] = 1;
+    info->m_J1linearAxis[2 * s + 2] = 1;
+	btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin();
+	{
+		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s);
+		btVector3 a1neg = -a1;
+		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+	btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
+	{
+		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s);
+		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+	// linear RHS
+    btScalar k = info->fps * info->erp;
+	for(i = 0; i < 3; i++)
+    {
+        info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]);
+    }
+	// make rotations around X and Y equal
+	// the hinge axis should be the only unconstrained
+	// rotational axis, the angular velocity of the two bodies perpendicular to
+	// the hinge axis should be equal. thus the constraint equations are
+	//    p*w1 - p*w2 = 0
+	//    q*w1 - q*w2 = 0
+	// where p and q are unit vectors normal to the hinge axis, and w1 and w2
+	// are the angular velocity vectors of the two bodies.
+	// get hinge axis (Z)
+	btVector3 ax1 = trA.getBasis().getColumn(2);
+	// get 2 orthos to hinge axis (X, Y)
+	btVector3 p = trA.getBasis().getColumn(0);
+	btVector3 q = trA.getBasis().getColumn(1);
+	// set the two hinge angular rows 
+    int s3 = 3 * info->rowskip;
+    int s4 = 4 * info->rowskip;
+
+	info->m_J1angularAxis[s3 + 0] = p[0];
+	info->m_J1angularAxis[s3 + 1] = p[1];
+	info->m_J1angularAxis[s3 + 2] = p[2];
+	info->m_J1angularAxis[s4 + 0] = q[0];
+	info->m_J1angularAxis[s4 + 1] = q[1];
+	info->m_J1angularAxis[s4 + 2] = q[2];
+
+	info->m_J2angularAxis[s3 + 0] = -p[0];
+	info->m_J2angularAxis[s3 + 1] = -p[1];
+	info->m_J2angularAxis[s3 + 2] = -p[2];
+	info->m_J2angularAxis[s4 + 0] = -q[0];
+	info->m_J2angularAxis[s4 + 1] = -q[1];
+	info->m_J2angularAxis[s4 + 2] = -q[2];
+    // compute the right hand side of the constraint equation. set relative
+    // body velocities along p and q to bring the hinge back into alignment.
+    // if ax1,ax2 are the unit length hinge axes as computed from body1 and
+    // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
+    // if `theta' is the angle between ax1 and ax2, we need an angular velocity
+    // along u to cover angle erp*theta in one step :
+    //   |angular_velocity| = angle/time = erp*theta / stepsize
+    //                      = (erp*fps) * theta
+    //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+    //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+    // ...as ax1 and ax2 are unit length. if theta is smallish,
+    // theta ~= sin(theta), so
+    //    angular_velocity  = (erp*fps) * (ax1 x ax2)
+    // ax1 x ax2 is in the plane space of ax1, so we project the angular
+    // velocity to p and q to find the right hand side.
+    btVector3 ax2 = - trB.getBasis().getColumn(2);
+	btVector3 u = ax1.cross(ax2);
+	info->m_constraintError[s3] = k * u.dot(p);
+	info->m_constraintError[s4] = k * u.dot(q);
+	// check angular limits
+	int nrow = 4; // last filled row
+	int srow;
+	btScalar limit_err = btScalar(0.0);
+	int limit = 0;
+	if(getSolveLimit())
+	{
+		limit_err = m_correction;
+		limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+	}
+	// if the hinge has joint limits or motor, add in the extra row
+	int powered = 0;
+	if(getEnableAngularMotor())
+	{
+		powered = 1;
+	}
+	if(limit || powered) 
+	{
+		nrow++;
+		srow = nrow * info->rowskip;
+		info->m_J1angularAxis[srow+0] = ax1[0];
+		info->m_J1angularAxis[srow+1] = ax1[1];
+		info->m_J1angularAxis[srow+2] = ax1[2];
+
+		info->m_J2angularAxis[srow+0] = -ax1[0];
+		info->m_J2angularAxis[srow+1] = -ax1[1];
+		info->m_J2angularAxis[srow+2] = -ax1[2];
+
+		btScalar lostop = getLowerLimit();
+		btScalar histop = getUpperLimit();
+		if(limit && (lostop == histop))
+		{  // the joint motor is ineffective
+			powered = 0;
+		}
+		if(powered)
+		{
+            info->cfm[srow] = btScalar(0.0); 
+			btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
+			info->m_constraintError[srow] = mot_fact * m_motorTargetVelocity;
+			info->m_lowerLimit[srow] = - m_maxMotorImpulse;
+			info->m_upperLimit[srow] =   m_maxMotorImpulse;
+		}
+		if(limit)
+		{
+			k = info->fps * info->erp;
+			info->m_constraintError[srow] = k * limit_err;
+			info->cfm[srow] = btScalar(0.0);
+			if(lostop == histop) 
+			{
+				// limited low and high simultaneously
+				info->m_lowerLimit[srow] = -SIMD_INFINITY;
+				info->m_upperLimit[srow] = SIMD_INFINITY;
+			}
+			else if(limit == 1) 
+			{ // low limit
+				info->m_lowerLimit[srow] = 0;
+				info->m_upperLimit[srow] = SIMD_INFINITY;
+			}
+			else 
+			{ // high limit
+				info->m_lowerLimit[srow] = -SIMD_INFINITY;
+				info->m_upperLimit[srow] = 0;
+			}
+			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
+			btScalar bounce = m_relaxationFactor;
+			if(bounce > btScalar(0.0))
+			{
+				btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
+				vel -= m_rbB.getAngularVelocity().dot(ax1);
+				// only apply bounce if the velocity is incoming, and if the
+				// resulting c[] exceeds what we already have.
+				if(limit == 1)
+				{	// low limit
+					if(vel < 0)
+					{
+						btScalar newc = -bounce * vel;
+						if(newc > info->m_constraintError[srow])
+						{
+							info->m_constraintError[srow] = newc;
+						}
+					}
+				}
+				else
+				{	// high limit - all those computations are reversed
+					if(vel > 0)
+					{
+						btScalar newc = -bounce * vel;
+						if(newc < info->m_constraintError[srow])
+						{
+							info->m_constraintError[srow] = newc;
+						}
+					}
+				}
+			}
+			info->m_constraintError[srow] *= m_biasFactor;
+		} // if(limit)
+	} // if angular limit or powered
 }
 
+//-----------------------------------------------------------------------------
 
 void	btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
 {
@@ -435,12 +616,16 @@ void	btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody
 
 }
 
+//-----------------------------------------------------------------------------
+
 void	btHingeConstraint::updateRHS(btScalar	timeStep)
 {
 	(void)timeStep;
 
 }
 
+//-----------------------------------------------------------------------------
+
 btScalar btHingeConstraint::getHingeAngle()
 {
 	const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
@@ -450,3 +635,33 @@ btScalar btHingeConstraint::getHingeAngle()
 	return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
 }
 
+//-----------------------------------------------------------------------------
+
+void btHingeConstraint::testLimit()
+{
+	// Compute limit information
+	m_hingeAngle = getHingeAngle();  
+	m_correction = btScalar(0.);
+	m_limitSign = btScalar(0.);
+	m_solveLimit = false;
+	if (m_lowerLimit <= m_upperLimit)
+	{
+		if (m_hingeAngle <= m_lowerLimit)
+		{
+			m_correction = (m_lowerLimit - m_hingeAngle);
+			m_limitSign = 1.0f;
+			m_solveLimit = true;
+		} 
+		else if (m_hingeAngle >= m_upperLimit)
+		{
+			m_correction = m_upperLimit - m_hingeAngle;
+			m_limitSign = -1.0f;
+			m_solveLimit = true;
+		}
+	}
+	return;
+} // btHingeConstraint::testLimit()
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------

src/BulletDynamics/ConstraintSolver/btHingeConstraint.h

@@ -53,6 +53,7 @@ public:
 	btScalar	m_correction;
 
 	btScalar	m_accLimitImpulse;
+	btScalar	m_hingeAngle;
 
 	bool		m_angularOnly;
 	bool		m_enableAngularMotor;
@@ -127,6 +128,8 @@ public:
 
 	btScalar getHingeAngle();
 
+	void testLimit();
+
 
 	const btTransform& getAFrame() { return m_rbAFrame; };	
 	const btTransform& getBFrame() { return m_rbBFrame; };

src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp

@@ -68,7 +68,9 @@ void btSliderConstraint::initParams()
 
 btSliderConstraint::btSliderConstraint()
         :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
-		m_useLinearReferenceFrameA(true)
+		m_useLinearReferenceFrameA(true),
+		m_useSolveConstraintObsolete(false)
+//		m_useSolveConstraintObsolete(true)
 {
 	initParams();
 } // btSliderConstraint::btSliderConstraint()
@@ -79,7 +81,9 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const
         : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB)
         , m_frameInA(frameInA)
         , m_frameInB(frameInB),
-		m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+		m_useLinearReferenceFrameA(useLinearReferenceFrameA),
+		m_useSolveConstraintObsolete(false)
+//		m_useSolveConstraintObsolete(true)
 {
 	initParams();
 } // btSliderConstraint::btSliderConstraint()
@@ -88,6 +92,10 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const
 
 void btSliderConstraint::buildJacobian()
 {
+	if (!m_useSolveConstraintObsolete) 
+	{
+		return;
+	}
 	if(m_useLinearReferenceFrameA)
 	{
 		buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB);
@@ -155,28 +163,358 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co
 
 //-----------------------------------------------------------------------------
 
-void btSliderConstraint::getInfo1 (btConstraintInfo1* info)
+void btSliderConstraint::getInfo1(btConstraintInfo1* info)
 {
-	info->m_numConstraintRows = 0;
+	if (m_useSolveConstraintObsolete)
-	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,bodyA, m_rbB,bodyB);
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
 	}
 	else
 	{
-		solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
+		info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
+		info->nub = 2; 
+		//prepare constraint
+		calculateTransforms();
+		testLinLimits();
+		if(getSolveLinLimit() || getPoweredLinMotor())
+		{
+			info->m_numConstraintRows++; // limit 3rd linear as well
+			info->nub--; 
+		}
+		testAngLimits();
+		if(getSolveAngLimit() || getPoweredAngMotor())
+		{
+			info->m_numConstraintRows++; // limit 3rd angular as well
+			info->nub--; 
+		}
+	}
+} // btSliderConstraint::getInfo1()
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::getInfo2(btConstraintInfo2* info)
+{
+	btAssert(!m_useSolveConstraintObsolete);
+	int i, s = info->rowskip;
+	const btTransform& trA = getCalculatedTransformA();
+	const btTransform& trB = getCalculatedTransformB();
+	btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
+	// make rotations around Y and Z equal
+	// the slider axis should be the only unconstrained
+	// rotational axis, the angular velocity of the two bodies perpendicular to
+	// the slider axis should be equal. thus the constraint equations are
+	//    p*w1 - p*w2 = 0
+	//    q*w1 - q*w2 = 0
+	// where p and q are unit vectors normal to the slider axis, and w1 and w2
+	// are the angular velocity vectors of the two bodies.
+	// get slider axis (X)
+	btVector3 ax1 = trA.getBasis().getColumn(0);
+	// get 2 orthos to slider axis (Y, Z)
+	btVector3 p = trA.getBasis().getColumn(1);
+	btVector3 q = trA.getBasis().getColumn(2);
+	// set the two slider rows 
+	info->m_J1angularAxis[0] = p[0];
+	info->m_J1angularAxis[1] = p[1];
+	info->m_J1angularAxis[2] = p[2];
+	info->m_J1angularAxis[s+0] = q[0];
+	info->m_J1angularAxis[s+1] = q[1];
+	info->m_J1angularAxis[s+2] = q[2];
+
+	info->m_J2angularAxis[0] = -p[0];
+	info->m_J2angularAxis[1] = -p[1];
+	info->m_J2angularAxis[2] = -p[2];
+	info->m_J2angularAxis[s+0] = -q[0];
+	info->m_J2angularAxis[s+1] = -q[1];
+	info->m_J2angularAxis[s+2] = -q[2];
+	// compute the right hand side of the constraint equation. set relative
+	// body velocities along p and q to bring the slider back into alignment.
+	// if ax1,ax2 are the unit length slider axes as computed from body1 and
+	// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
+	// if "theta" is the angle between ax1 and ax2, we need an angular velocity
+	// along u to cover angle erp*theta in one step :
+	//   |angular_velocity| = angle/time = erp*theta / stepsize
+	//                      = (erp*fps) * theta
+	//    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+	//                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+	// ...as ax1 and ax2 are unit length. if theta is smallish,
+	// theta ~= sin(theta), so
+	//    angular_velocity  = (erp*fps) * (ax1 x ax2)
+	// ax1 x ax2 is in the plane space of ax1, so we project the angular
+	// velocity to p and q to find the right hand side.
+	btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
+    btVector3 ax2 = trB.getBasis().getColumn(0);
+	btVector3 u = ax1.cross(ax2);
+	info->m_constraintError[0] = k * u.dot(p);
+	info->m_constraintError[s] = k * u.dot(q);
+	// pull out pos and R for both bodies. also get the connection
+	// vector c = pos2-pos1.
+	// next two rows. we want: vel2 = vel1 + w1 x c ... but this would
+	// result in three equations, so we project along the planespace vectors
+	// so that sliding along the slider axis is disregarded. for symmetry we
+	// also consider rotation around center of mass of two bodies (factA and factB).
+	btTransform bodyA_trans = m_rbA.getCenterOfMassTransform();
+	btTransform bodyB_trans = m_rbB.getCenterOfMassTransform();
+	int s2 = 2 * s, s3 = 3 * s;
+	btVector3 c;
+	btScalar miA = m_rbA.getInvMass();
+	btScalar miB = m_rbB.getInvMass();
+	btScalar miS = miA + miB;
+	btScalar factA, factB;
+	if(miS > btScalar(0.f))
+	{
+		factA = miB / miS;
+	}
+	else 
+	{
+		factA = btScalar(0.5f);
+	}
+	if(factA > 0.99f) factA = 0.99f;
+	if(factA < 0.01f) factA = 0.01f;
+	factB = btScalar(1.0f) - factA;
+	c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
+	btVector3 tmp = c.cross(p);
+	for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
+	for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
+	tmp = c.cross(q);
+	for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
+	for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
+
+	for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
+	for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
+	// compute two elements of right hand side. we want to align the offset
+	// point (in body 2's frame) with the center of body 1.
+	btVector3 ofs; // offset point in global coordinates
+	ofs = trB.getOrigin() - trA.getOrigin();
+	k = info->fps * info->erp * getSoftnessOrthoLin();
+	info->m_constraintError[s2] = k * p.dot(ofs);
+	info->m_constraintError[s3] = k * q.dot(ofs);
+	int nrow = 3; // last filled row
+	int srow;
+	// check linear limits linear
+	btScalar limit_err = btScalar(0.0);
+	int limit = 0;
+	if(getSolveLinLimit())
+	{
+		limit_err = getLinDepth() *  signFact;
+		limit = (limit_err > btScalar(0.0)) ? 2 : 1;
+	}
+	int powered = 0;
+	if(getPoweredLinMotor())
+	{
+		powered = 1;
+	}
+	// if the slider has joint limits or motor, add in the extra row
+	if (limit || powered) 
+	{
+		nrow++;
+		srow = nrow * info->rowskip;
+		info->m_J1linearAxis[srow+0] = ax1[0];
+		info->m_J1linearAxis[srow+1] = ax1[1];
+		info->m_J1linearAxis[srow+2] = ax1[2];
+		// linear torque decoupling step:
+		//
+		// we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
+		// do not create a torque couple. in other words, the points that the
+		// constraint force is applied at must lie along the same ax1 axis.
+		// a torque couple will result in limited slider-jointed free
+		// bodies from gaining angular momentum.
+		// the solution used here is to apply the constraint forces at the center of mass of the two bodies
+		btVector3 ltd;	// Linear Torque Decoupling vector (a torque)
+//		c = btScalar(0.5) * c;
+		ltd = c.cross(ax1);
+		info->m_J1angularAxis[srow+0] = factA*ltd[0];
+		info->m_J1angularAxis[srow+1] = factA*ltd[1];
+		info->m_J1angularAxis[srow+2] = factA*ltd[2];
+		info->m_J2angularAxis[srow+0] = factB*ltd[0];
+		info->m_J2angularAxis[srow+1] = factB*ltd[1];
+		info->m_J2angularAxis[srow+2] = factB*ltd[2];
+		// right-hand part
+		btScalar lostop = getLowerLinLimit();
+		btScalar histop = getUpperLinLimit();
+		if(limit && (lostop == histop))
+		{  // the joint motor is ineffective
+			powered = 0;
+		}
+		info->m_constraintError[srow] = 0.;
+		info->m_lowerLimit[srow] = 0.;
+		info->m_upperLimit[srow] = 0.;
+		if(powered)
+		{
+            info->cfm[nrow] = btScalar(0.0); 
+			btScalar tag_vel = getTargetLinMotorVelocity();
+			btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp);
+			info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity();
+			info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
+			info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
+		}
+		if(limit)
+		{
+			k = info->fps * info->erp;
+			info->m_constraintError[srow] += k * limit_err;
+			info->cfm[srow] = btScalar(0.0); // stop_cfm;
+			if(lostop == histop) 
+			{	// limited low and high simultaneously
+				info->m_lowerLimit[srow] = -SIMD_INFINITY;
+				info->m_upperLimit[srow] = SIMD_INFINITY;
+			}
+			else if(limit == 1) 
+			{ // low limit
+				info->m_lowerLimit[srow] = -SIMD_INFINITY;
+				info->m_upperLimit[srow] = 0;
+			}
+			else 
+			{ // high limit
+				info->m_lowerLimit[srow] = 0;
+				info->m_upperLimit[srow] = SIMD_INFINITY;
+			}
+			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
+			btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
+			if(bounce > btScalar(0.0))
+			{
+				btScalar vel = m_rbA.getLinearVelocity().dot(ax1);
+				vel -= m_rbB.getLinearVelocity().dot(ax1);
+				vel *= signFact;
+				// only apply bounce if the velocity is incoming, and if the
+				// resulting c[] exceeds what we already have.
+				if(limit == 1)
+				{	// low limit
+					if(vel < 0)
+					{
+						btScalar newc = -bounce * vel;
+						if (newc > info->m_constraintError[srow])
+						{
+							info->m_constraintError[srow] = newc;
+						}
+					}
+				}
+				else
+				{ // high limit - all those computations are reversed
+					if(vel > 0)
+					{
+						btScalar newc = -bounce * vel;
+						if(newc < info->m_constraintError[srow]) 
+						{
+							info->m_constraintError[srow] = newc;
+						}
+					}
+				}
+			}
+			info->m_constraintError[srow] *= getSoftnessLimLin();
+		} // if(limit)
+	} // if linear limit
+	// check angular limits
+	limit_err = btScalar(0.0);
+	limit = 0;
+	if(getSolveAngLimit())
+	{
+		limit_err = getAngDepth();
+		limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+	}
+	// if the slider has joint limits, add in the extra row
+	powered = 0;
+	if(getPoweredAngMotor())
+	{
+		powered = 1;
+	}
+	if(limit || powered) 
+	{
+		nrow++;
+		srow = nrow * info->rowskip;
+		info->m_J1angularAxis[srow+0] = ax1[0];
+		info->m_J1angularAxis[srow+1] = ax1[1];
+		info->m_J1angularAxis[srow+2] = ax1[2];
+
+		info->m_J2angularAxis[srow+0] = -ax1[0];
+		info->m_J2angularAxis[srow+1] = -ax1[1];
+		info->m_J2angularAxis[srow+2] = -ax1[2];
+
+		btScalar lostop = getLowerAngLimit();
+		btScalar histop = getUpperAngLimit();
+		if(limit && (lostop == histop))
+		{  // the joint motor is ineffective
+			powered = 0;
+		}
+		if(powered)
+		{
+            info->cfm[srow] = btScalar(0.0); 
+			btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
+			info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
+			info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
+			info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
+		}
+		if(limit)
+		{
+			k = info->fps * info->erp;
+			info->m_constraintError[srow] += k * limit_err;
+			info->cfm[srow] = btScalar(0.0); // stop_cfm;
+			if(lostop == histop) 
+			{
+				// limited low and high simultaneously
+				info->m_lowerLimit[srow] = -SIMD_INFINITY;
+				info->m_upperLimit[srow] = SIMD_INFINITY;
+			}
+			else if(limit == 1) 
+			{ // low limit
+				info->m_lowerLimit[srow] = 0;
+				info->m_upperLimit[srow] = SIMD_INFINITY;
+			}
+			else 
+			{ // high limit
+				info->m_lowerLimit[srow] = -SIMD_INFINITY;
+				info->m_upperLimit[srow] = 0;
+			}
+			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
+			btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
+			if(bounce > btScalar(0.0))
+			{
+				btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
+				vel -= m_rbB.getAngularVelocity().dot(ax1);
+				// only apply bounce if the velocity is incoming, and if the
+				// resulting c[] exceeds what we already have.
+				if(limit == 1)
+				{	// low limit
+					if(vel < 0)
+					{
+						btScalar newc = -bounce * vel;
+						if(newc > info->m_constraintError[srow])
+						{
+							info->m_constraintError[srow] = newc;
+						}
+					}
+				}
+				else
+				{	// high limit - all those computations are reversed
+					if(vel > 0)
+					{
+						btScalar newc = -bounce * vel;
+						if(newc < info->m_constraintError[srow])
+						{
+							info->m_constraintError[srow] = newc;
+						}
+					}
+				}
+			}
+			info->m_constraintError[srow] *= getSoftnessLimAng();
+		} // if(limit)
+	} // if angular limit or powered
+} // btSliderConstraint::getInfo2()
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		m_timeStep = timeStep;
+		if(m_useLinearReferenceFrameA)
+		{
+			solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB);
+		}
+		else
+		{
+			solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
+		}
 	}
 } // btSliderConstraint::solveConstraint()
 
@@ -371,7 +709,7 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& body
 //-----------------------------------------------------------------------------
 
 void btSliderConstraint::calculateTransforms(void){
-	if(m_useLinearReferenceFrameA)
+	if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
 	{
 		m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
 		m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
@@ -384,7 +722,14 @@ void btSliderConstraint::calculateTransforms(void){
 	m_realPivotAInW = m_calculatedTransformA.getOrigin();
 	m_realPivotBInW = m_calculatedTransformB.getOrigin();
 	m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
-	m_delta = m_realPivotBInW - m_realPivotAInW;
+	if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
+	{
+		m_delta = m_realPivotBInW - m_realPivotAInW;
+	}
+	else
+	{
+		m_delta = m_realPivotAInW - m_realPivotBInW;
+	}
 	m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
     btVector3 normalWorld;
     int i;
@@ -426,7 +771,6 @@ void btSliderConstraint::testLinLimits(void)
 } // btSliderConstraint::testLinLimits()
 
 //-----------------------------------------------------------------------------
- 
 
 void btSliderConstraint::testAngLimits(void)
 {
@@ -438,6 +782,7 @@ void btSliderConstraint::testAngLimits(void)
 		const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
 		const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
 		btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));  
+		m_angPos = rot;
 		if(rot < m_lowerAngLimit)
 		{
 			m_angDepth = rot - m_lowerAngLimit;
@@ -450,12 +795,9 @@ void btSliderConstraint::testAngLimits(void)
 		}
 	}
 } // btSliderConstraint::testAngLimits()
-
 	
 //-----------------------------------------------------------------------------
 
-
-
 btVector3 btSliderConstraint::getAncorInA(void)
 {
 	btVector3 ancorInA;

src/BulletDynamics/ConstraintSolver/btSliderConstraint.h

@@ -46,6 +46,8 @@ class btRigidBody;
 class btSliderConstraint : public btTypedConstraint
 {
 protected:
+	///for backwards compatibility during the transition to 'getInfo/getInfo2'
+	bool		m_useSolveConstraintObsolete;
 	btTransform	m_frameInA;
     btTransform	m_frameInB;
 	// use frameA fo define limits, if true
@@ -104,6 +106,7 @@ protected:
 	btVector3 m_relPosB;
 
 	btScalar m_linPos;
+	btScalar m_angPos;
 
 	btScalar m_angDepth;
 	btScalar m_kAngle;
@@ -200,6 +203,7 @@ public:
 	void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; }
 	btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
 	btScalar getLinearPos() { return m_linPos; }
+	
 
 	// access for ODE solver
 	bool getSolveLinLimit() { return m_solveLinLim; }
@@ -212,6 +216,7 @@ public:
 	// shared code used by ODE solver
 	void	calculateTransforms(void);
 	void	testLinLimits(void);
+	void	testLinLimits2(btConstraintInfo2* info);
 	void	testAngLimits(void);
 	// access for PE Solver
 	btVector3 getAncorInA(void);

src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp

@@ -54,3 +54,56 @@ m_appliedImpulse(btScalar(0.))
 
 }
 
+
+//-----------------------------------------------------------------------------
+
+btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
+{
+	if(lowLim > uppLim)
+	{
+		return btScalar(1.0f);
+	}
+	else if(lowLim == uppLim)
+	{
+		return btScalar(0.0f);
+	}
+	btScalar lim_fact = btScalar(1.0f);
+	btScalar delta_max = vel / timeFact;
+	if(delta_max > btScalar(0.0f))
+	{
+		if((pos > lowLim) && (pos < (lowLim + delta_max)))
+		{
+			lim_fact = (pos  - lowLim) / delta_max;
+		}
+		else if(pos  < lowLim)
+		{
+			lim_fact = btScalar(0.0f);
+		}
+		else
+		{
+			lim_fact = btScalar(1.0f);
+		}
+	}
+	else if(delta_max < btScalar(0.0f))
+	{
+		if((pos < uppLim) && (pos > (uppLim + delta_max)))
+		{
+			lim_fact = (pos - uppLim) / delta_max;
+		}
+		else if(pos  > uppLim)
+		{
+			lim_fact = btScalar(0.0f);
+		}
+		else
+		{
+			lim_fact = btScalar(1.0f);
+		}
+	}
+	else
+	{
+			lim_fact = btScalar(0.0f);
+	}
+	return lim_fact;
+} // btTypedConstraint::getMotorFactor()
+
+

src/BulletDynamics/ConstraintSolver/btTypedConstraint.h

@@ -21,6 +21,9 @@ class btRigidBody;
 #include "btSolverConstraint.h"
 struct  btSolverBody;
 
+
+
+
 enum btTypedConstraintType
 {
 	POINT2POINT_CONSTRAINT_TYPE,
@@ -103,6 +106,7 @@ public:
 
 	virtual	void	solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep) = 0;
 
+	btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
 	
 	const btRigidBody& getRigidBodyA() const
 	{

src/BulletDynamics/Dynamics/btRigidBody.h

@@ -232,6 +232,16 @@ public:
 	{
 		m_totalForce += force;
 	}
+
+	const btVector3& getTotalForce()
+	{
+		return m_totalForce;
+	};
+
+	const btVector3& getTotalTorque()
+	{
+		return m_totalTorque;
+	};
     
 	const btVector3& getInvInertiaDiagLocal()
 	{