Index: code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
===================================================================
--- code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp	(revision 2777)
+++ code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp	(revision 2907)
@@ -20,17 +20,31 @@
 #include "LinearMath/btMinMax.h"
 #include <new>
+#include "btSolverBody.h"
+
+//-----------------------------------------------------------------------------
+
+#define HINGE_USE_OBSOLETE_SOLVER false
+
+//-----------------------------------------------------------------------------
 
 
 btHingeConstraint::btHingeConstraint()
 : btTypedConstraint (HINGE_CONSTRAINT_TYPE),
-m_enableAngularMotor(false)
-{
-}
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(false)
+{
+	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+
+//-----------------------------------------------------------------------------
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
-									 btVector3& axisInA,btVector3& axisInB)
+									 btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
 									 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
 									 m_angularOnly(false),
-									 m_enableAngularMotor(false)
+									 m_enableAngularMotor(false),
+									 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+									 m_useReferenceFrameA(useReferenceFrameA)
 {
 	m_rbAFrame.getOrigin() = pivotInA;
@@ -61,7 +75,7 @@
 	
 	m_rbBFrame.getOrigin() = pivotInB;
-	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
-									rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
-									rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
+	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
+									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
+									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
 	
 	//start with free
@@ -72,11 +86,13 @@
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
-
-}
-
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
-:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
+	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+
+//-----------------------------------------------------------------------------
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), 
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
 {
 
@@ -91,5 +107,5 @@
 									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
 
-	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
+	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
 
 	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
@@ -110,17 +126,17 @@
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
-}
+	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+
+//-----------------------------------------------------------------------------
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
-								     const btTransform& rbAFrame, const btTransform& rbBFrame)
+								     const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
 m_angularOnly(false),
-m_enableAngularMotor(false)
-{
-	// flip axis
-	m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
-
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
+{
 	//start with free
 	m_lowerLimit = btScalar(1e30);
@@ -130,19 +146,17 @@
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
+	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }			
 
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
+//-----------------------------------------------------------------------------
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
 m_angularOnly(false),
-m_enableAngularMotor(false)
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
 {
 	///not providing rigidbody B means implicitly using worldspace for body B
-
-	// flip axis
-	m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
 
 	m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
@@ -155,31 +169,36 @@
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
-}
+	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
+}
+
+//-----------------------------------------------------------------------------
 
 void	btHingeConstraint::buildJacobian()
 {
-	m_appliedImpulse = btScalar(0.);
-
-	if (!m_angularOnly)
-	{
-		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
-		btVector3 relPos = pivotBInW - pivotAInW;
-
-		btVector3 normal[3];
-		if (relPos.length2() > SIMD_EPSILON)
-		{
-			normal[0] = relPos.normalized();
-		}
-		else
-		{
-			normal[0].setValue(btScalar(1.0),0,0);
-		}
-
-		btPlaneSpace1(normal[0], normal[1], normal[2]);
-
-		for (int i=0;i<3;i++)
-		{
-			new (&m_jac[i]) btJacobianEntry(
+	if (m_useSolveConstraintObsolete)
+	{
+		m_appliedImpulse = btScalar(0.);
+
+		if (!m_angularOnly)
+		{
+			btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+			btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+			btVector3 relPos = pivotBInW - pivotAInW;
+
+			btVector3 normal[3];
+			if (relPos.length2() > SIMD_EPSILON)
+			{
+				normal[0] = relPos.normalized();
+			}
+			else
+			{
+				normal[0].setValue(btScalar(1.0),0,0);
+			}
+
+			btPlaneSpace1(normal[0], normal[1], normal[2]);
+
+			for (int i=0;i<3;i++)
+			{
+				new (&m_jac[i]) btJacobianEntry(
 				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
 				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -191,212 +210,457 @@
 				m_rbB.getInvInertiaDiagLocal(),
 				m_rbB.getInvMass());
+			}
 		}
-	}
-
-	//calculate two perpendicular jointAxis, orthogonal to hingeAxis
-	//these two jointAxis require equal angular velocities for both bodies
-
-	//this is unused for now, it's a todo
-	btVector3 jointAxis0local;
-	btVector3 jointAxis1local;
-	
-	btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
-
-	getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
-	btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
-	btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
-	btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+
+		//calculate two perpendicular jointAxis, orthogonal to hingeAxis
+		//these two jointAxis require equal angular velocities for both bodies
+
+		//this is unused for now, it's a todo
+		btVector3 jointAxis0local;
+		btVector3 jointAxis1local;
 		
-	new (&m_jacAng[0])	btJacobianEntry(jointAxis0,
-		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-		m_rbA.getInvInertiaDiagLocal(),
-		m_rbB.getInvInertiaDiagLocal());
-
-	new (&m_jacAng[1])	btJacobianEntry(jointAxis1,
-		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-		m_rbA.getInvInertiaDiagLocal(),
-		m_rbB.getInvInertiaDiagLocal());
-
-	new (&m_jacAng[2])	btJacobianEntry(hingeAxisWorld,
-		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-		m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-		m_rbA.getInvInertiaDiagLocal(),
-		m_rbB.getInvInertiaDiagLocal());
-
-
+		btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
+
+		getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+		btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
+		btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
+		btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+			
+		new (&m_jacAng[0])	btJacobianEntry(jointAxis0,
+			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+			m_rbA.getInvInertiaDiagLocal(),
+			m_rbB.getInvInertiaDiagLocal());
+
+		new (&m_jacAng[1])	btJacobianEntry(jointAxis1,
+			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+			m_rbA.getInvInertiaDiagLocal(),
+			m_rbB.getInvInertiaDiagLocal());
+
+		new (&m_jacAng[2])	btJacobianEntry(hingeAxisWorld,
+			m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+			m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+			m_rbA.getInvInertiaDiagLocal(),
+			m_rbB.getInvInertiaDiagLocal());
+
+			// clear accumulator
+			m_accLimitImpulse = btScalar(0.);
+
+			// test angular limit
+			testLimit();
+
+		//Compute K = J*W*J' for hinge axis
+		btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
+		m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
+							 getRigidBodyB().computeAngularImpulseDenominator(axisA));
+
+	}
+}
+
+//-----------------------------------------------------------------------------
+
+void btHingeConstraint::getInfo1(btConstraintInfo1* info)
+{
+	if (m_useSolveConstraintObsolete)
+	{
+		info->m_numConstraintRows = 0;
+		info->nub = 0;
+	}
+	else
+	{
+		info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
+		info->nub = 1; 
+		//prepare constraint
+		testLimit();
+		if(getSolveLimit() || getEnableAngularMotor())
+		{
+			info->m_numConstraintRows++; // limit 3rd anguar as well
+			info->nub--; 
+		}
+	}
+} // btHingeConstraint::getInfo1 ()
+
+//-----------------------------------------------------------------------------
+
+void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
+{
+	btAssert(!m_useSolveConstraintObsolete);
+	int i, s = info->rowskip;
+	// transforms in world space
+	btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
+	btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
+	// pivot point
+	btVector3 pivotAInW = trA.getOrigin();
+	btVector3 pivotBInW = trB.getOrigin();
+	// linear (all fixed)
+    info->m_J1linearAxis[0] = 1;
+    info->m_J1linearAxis[s + 1] = 1;
+    info->m_J1linearAxis[2 * s + 2] = 1;
+	btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin();
+	{
+		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
+		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s);
+		btVector3 a1neg = -a1;
+		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+	btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
+	{
+		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
+		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s);
+		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+	}
+	// linear RHS
+    btScalar k = info->fps * info->erp;
+	for(i = 0; i < 3; i++)
+    {
+        info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]);
+    }
+	// make rotations around X and Y equal
+	// the hinge axis should be the only unconstrained
+	// rotational axis, the angular velocity of the two bodies perpendicular to
+	// the hinge axis should be equal. thus the constraint equations are
+	//    p*w1 - p*w2 = 0
+	//    q*w1 - q*w2 = 0
+	// where p and q are unit vectors normal to the hinge axis, and w1 and w2
+	// are the angular velocity vectors of the two bodies.
+	// get hinge axis (Z)
+	btVector3 ax1 = trA.getBasis().getColumn(2);
+	// get 2 orthos to hinge axis (X, Y)
+	btVector3 p = trA.getBasis().getColumn(0);
+	btVector3 q = trA.getBasis().getColumn(1);
+	// set the two hinge angular rows 
+    int s3 = 3 * info->rowskip;
+    int s4 = 4 * info->rowskip;
+
+	info->m_J1angularAxis[s3 + 0] = p[0];
+	info->m_J1angularAxis[s3 + 1] = p[1];
+	info->m_J1angularAxis[s3 + 2] = p[2];
+	info->m_J1angularAxis[s4 + 0] = q[0];
+	info->m_J1angularAxis[s4 + 1] = q[1];
+	info->m_J1angularAxis[s4 + 2] = q[2];
+
+	info->m_J2angularAxis[s3 + 0] = -p[0];
+	info->m_J2angularAxis[s3 + 1] = -p[1];
+	info->m_J2angularAxis[s3 + 2] = -p[2];
+	info->m_J2angularAxis[s4 + 0] = -q[0];
+	info->m_J2angularAxis[s4 + 1] = -q[1];
+	info->m_J2angularAxis[s4 + 2] = -q[2];
+    // compute the right hand side of the constraint equation. set relative
+    // body velocities along p and q to bring the hinge back into alignment.
+    // if ax1,ax2 are the unit length hinge axes as computed from body1 and
+    // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
+    // if `theta' is the angle between ax1 and ax2, we need an angular velocity
+    // along u to cover angle erp*theta in one step :
+    //   |angular_velocity| = angle/time = erp*theta / stepsize
+    //                      = (erp*fps) * theta
+    //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+    //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+    // ...as ax1 and ax2 are unit length. if theta is smallish,
+    // theta ~= sin(theta), so
+    //    angular_velocity  = (erp*fps) * (ax1 x ax2)
+    // ax1 x ax2 is in the plane space of ax1, so we project the angular
+    // velocity to p and q to find the right hand side.
+    btVector3 ax2 = trB.getBasis().getColumn(2);
+	btVector3 u = ax1.cross(ax2);
+	info->m_constraintError[s3] = k * u.dot(p);
+	info->m_constraintError[s4] = k * u.dot(q);
+	// check angular limits
+	int nrow = 4; // last filled row
+	int srow;
+	btScalar limit_err = btScalar(0.0);
+	int limit = 0;
+	if(getSolveLimit())
+	{
+		limit_err = m_correction * m_referenceSign;
+		limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+	}
+	// if the hinge has joint limits or motor, add in the extra row
+	int powered = 0;
+	if(getEnableAngularMotor())
+	{
+		powered = 1;
+	}
+	if(limit || powered) 
+	{
+		nrow++;
+		srow = nrow * info->rowskip;
+		info->m_J1angularAxis[srow+0] = ax1[0];
+		info->m_J1angularAxis[srow+1] = ax1[1];
+		info->m_J1angularAxis[srow+2] = ax1[2];
+
+		info->m_J2angularAxis[srow+0] = -ax1[0];
+		info->m_J2angularAxis[srow+1] = -ax1[1];
+		info->m_J2angularAxis[srow+2] = -ax1[2];
+
+		btScalar lostop = getLowerLimit();
+		btScalar histop = getUpperLimit();
+		if(limit && (lostop == histop))
+		{  // the joint motor is ineffective
+			powered = 0;
+		}
+		info->m_constraintError[srow] = btScalar(0.0f);
+		if(powered)
+		{
+            info->cfm[srow] = btScalar(0.0); 
+			btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
+			info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
+			info->m_lowerLimit[srow] = - m_maxMotorImpulse;
+			info->m_upperLimit[srow] =   m_maxMotorImpulse;
+		}
+		if(limit)
+		{
+			k = info->fps * info->erp;
+			info->m_constraintError[srow] += k * limit_err;
+			info->cfm[srow] = btScalar(0.0);
+			if(lostop == histop) 
+			{
+				// limited low and high simultaneously
+				info->m_lowerLimit[srow] = -SIMD_INFINITY;
+				info->m_upperLimit[srow] = SIMD_INFINITY;
+			}
+			else if(limit == 1) 
+			{ // low limit
+				info->m_lowerLimit[srow] = 0;
+				info->m_upperLimit[srow] = SIMD_INFINITY;
+			}
+			else 
+			{ // high limit
+				info->m_lowerLimit[srow] = -SIMD_INFINITY;
+				info->m_upperLimit[srow] = 0;
+			}
+			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
+			btScalar bounce = m_relaxationFactor;
+			if(bounce > btScalar(0.0))
+			{
+				btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
+				vel -= m_rbB.getAngularVelocity().dot(ax1);
+				// only apply bounce if the velocity is incoming, and if the
+				// resulting c[] exceeds what we already have.
+				if(limit == 1)
+				{	// low limit
+					if(vel < 0)
+					{
+						btScalar newc = -bounce * vel;
+						if(newc > info->m_constraintError[srow])
+						{
+							info->m_constraintError[srow] = newc;
+						}
+					}
+				}
+				else
+				{	// high limit - all those computations are reversed
+					if(vel > 0)
+					{
+						btScalar newc = -bounce * vel;
+						if(newc < info->m_constraintError[srow])
+						{
+							info->m_constraintError[srow] = newc;
+						}
+					}
+				}
+			}
+			info->m_constraintError[srow] *= m_biasFactor;
+		} // if(limit)
+	} // if angular limit or powered
+}
+
+//-----------------------------------------------------------------------------
+
+void	btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
+{
+
+	///for backwards compatibility during the transition to 'getInfo/getInfo2'
+	if (m_useSolveConstraintObsolete)
+	{
+
+		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+
+		btScalar tau = btScalar(0.3);
+
+		//linear part
+		if (!m_angularOnly)
+		{
+			btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+			btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+
+			btVector3 vel1,vel2;
+			bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+			bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+			btVector3 vel = vel1 - vel2;
+
+			for (int i=0;i<3;i++)
+			{		
+				const btVector3& normal = m_jac[i].m_linearJointAxis;
+				btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+
+				btScalar rel_vel;
+				rel_vel = normal.dot(vel);
+				//positional error (zeroth order error)
+				btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+				btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
+				m_appliedImpulse += impulse;
+				btVector3 impulse_vector = normal * impulse;
+				btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
+				btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
+				bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
+				bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
+			}
+		}
+
+		
+		{
+			///solve angular part
+
+			// get axes in world space
+			btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
+			btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
+
+			btVector3 angVelA;
+			bodyA.getAngularVelocity(angVelA);
+			btVector3 angVelB;
+			bodyB.getAngularVelocity(angVelB);
+
+			btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
+			btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
+
+			btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
+			btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
+			btVector3 velrelOrthog = angAorthog-angBorthog;
+			{
+				
+
+				//solve orthogonal angular velocity correction
+				btScalar relaxation = btScalar(1.);
+				btScalar len = velrelOrthog.length();
+				if (len > btScalar(0.00001))
+				{
+					btVector3 normal = velrelOrthog.normalized();
+					btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
+						getRigidBodyB().computeAngularImpulseDenominator(normal);
+					// scale for mass and relaxation
+					//velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
+
+					bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom));
+					bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom));
+
+				}
+
+				//solve angular positional correction
+				btVector3 angularError =  axisA.cross(axisB) *(btScalar(1.)/timeStep);
+				btScalar len2 = angularError.length();
+				if (len2>btScalar(0.00001))
+				{
+					btVector3 normal2 = angularError.normalized();
+					btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
+							getRigidBodyB().computeAngularImpulseDenominator(normal2);
+					//angularError *= (btScalar(1.)/denom2) * relaxation;
+					
+					bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2));
+					bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2));
+
+				}
+				
+				
+
+
+
+				// solve limit
+				if (m_solveLimit)
+				{
+					btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
+
+					btScalar impulseMag = amplitude * m_kHinge;
+
+					// Clamp the accumulated impulse
+					btScalar temp = m_accLimitImpulse;
+					m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
+					impulseMag = m_accLimitImpulse - temp;
+
+
+					
+					bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign);
+					bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign));
+
+				}
+			}
+
+			//apply motor
+			if (m_enableAngularMotor) 
+			{
+				//todo: add limits too
+				btVector3 angularLimit(0,0,0);
+
+				btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
+				btScalar projRelVel = velrel.dot(axisA);
+
+				btScalar desiredMotorVel = m_motorTargetVelocity;
+				btScalar motor_relvel = desiredMotorVel - projRelVel;
+
+				btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
+				//todo: should clip against accumulated impulse
+				btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
+				clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
+				btVector3 motorImp = clippedMotorImpulse * axisA;
+			
+				bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
+				bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
+				
+			}
+		}
+	}
+
+}
+
+//-----------------------------------------------------------------------------
+
+void	btHingeConstraint::updateRHS(btScalar	timeStep)
+{
+	(void)timeStep;
+
+}
+
+//-----------------------------------------------------------------------------
+
+btScalar btHingeConstraint::getHingeAngle()
+{
+	const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
+	const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
+	const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
+	btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+	return m_referenceSign * angle;
+}
+
+//-----------------------------------------------------------------------------
+
+void btHingeConstraint::testLimit()
+{
 	// Compute limit information
-	btScalar hingeAngle = getHingeAngle();  
-
-	//set bias, sign, clear accumulator
+	m_hingeAngle = getHingeAngle();  
 	m_correction = btScalar(0.);
 	m_limitSign = btScalar(0.);
 	m_solveLimit = false;
-	m_accLimitImpulse = btScalar(0.);
-
-//	if (m_lowerLimit < m_upperLimit)
 	if (m_lowerLimit <= m_upperLimit)
 	{
-//		if (hingeAngle <= m_lowerLimit*m_limitSoftness)
-		if (hingeAngle <= m_lowerLimit)
-		{
-			m_correction = (m_lowerLimit - hingeAngle);
+		if (m_hingeAngle <= m_lowerLimit)
+		{
+			m_correction = (m_lowerLimit - m_hingeAngle);
 			m_limitSign = 1.0f;
 			m_solveLimit = true;
 		} 
-//		else if (hingeAngle >= m_upperLimit*m_limitSoftness)
-		else if (hingeAngle >= m_upperLimit)
-		{
-			m_correction = m_upperLimit - hingeAngle;
+		else if (m_hingeAngle >= m_upperLimit)
+		{
+			m_correction = m_upperLimit - m_hingeAngle;
 			m_limitSign = -1.0f;
 			m_solveLimit = true;
 		}
 	}
-
-	//Compute K = J*W*J' for hinge axis
-	btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
-	m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
-			             getRigidBodyB().computeAngularImpulseDenominator(axisA));
-
-}
-
-void	btHingeConstraint::solveConstraint(btScalar	timeStep)
-{
-
-	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
-
-	btScalar tau = btScalar(0.3);
-
-	//linear part
-	if (!m_angularOnly)
-	{
-		btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
-		btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-
-		btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-		btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-		btVector3 vel = vel1 - vel2;
-
-		for (int i=0;i<3;i++)
-		{		
-			const btVector3& normal = m_jac[i].m_linearJointAxis;
-			btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
-
-			btScalar rel_vel;
-			rel_vel = normal.dot(vel);
-			//positional error (zeroth order error)
-			btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
-			btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
-			m_appliedImpulse += impulse;
-			btVector3 impulse_vector = normal * impulse;
-			m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
-			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
-		}
-	}
-
-	
-	{
-		///solve angular part
-
-		// get axes in world space
-		btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
-		btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
-
-		const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
-		const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
-
-		btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
-		btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
-
-		btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
-		btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
-		btVector3 velrelOrthog = angAorthog-angBorthog;
-		{
-			//solve orthogonal angular velocity correction
-			btScalar relaxation = btScalar(1.);
-			btScalar len = velrelOrthog.length();
-			if (len > btScalar(0.00001))
-			{
-				btVector3 normal = velrelOrthog.normalized();
-				btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
-					getRigidBodyB().computeAngularImpulseDenominator(normal);
-				// scale for mass and relaxation
-				velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
-			}
-
-			//solve angular positional correction
-			btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep);
-			btScalar len2 = angularError.length();
-			if (len2>btScalar(0.00001))
-			{
-				btVector3 normal2 = angularError.normalized();
-				btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
-						getRigidBodyB().computeAngularImpulseDenominator(normal2);
-				angularError *= (btScalar(1.)/denom2) * relaxation;
-			}
-
-			m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
-			m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
-
-			// solve limit
-			if (m_solveLimit)
-			{
-				btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
-
-				btScalar impulseMag = amplitude * m_kHinge;
-
-				// Clamp the accumulated impulse
-				btScalar temp = m_accLimitImpulse;
-				m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
-				impulseMag = m_accLimitImpulse - temp;
-
-
-				btVector3 impulse = axisA * impulseMag * m_limitSign;
-				m_rbA.applyTorqueImpulse(impulse);
-				m_rbB.applyTorqueImpulse(-impulse);
-			}
-		}
-
-		//apply motor
-		if (m_enableAngularMotor) 
-		{
-			//todo: add limits too
-			btVector3 angularLimit(0,0,0);
-
-			btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
-			btScalar projRelVel = velrel.dot(axisA);
-
-			btScalar desiredMotorVel = m_motorTargetVelocity;
-			btScalar motor_relvel = desiredMotorVel - projRelVel;
-
-			btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
-			//todo: should clip against accumulated impulse
-			btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
-			clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
-			btVector3 motorImp = clippedMotorImpulse * axisA;
-
-			m_rbA.applyTorqueImpulse(motorImp+angularLimit);
-			m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
-			
-		}
-	}
-
-}
-
-void	btHingeConstraint::updateRHS(btScalar	timeStep)
-{
-	(void)timeStep;
-
-}
-
-btScalar btHingeConstraint::getHingeAngle()
-{
-	const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
-	const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
-	const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
-
-	return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
-}
-
+	return;
+} // btHingeConstraint::testLimit()
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
