reverted to old hinge constraint, it breaks several tests/demos

2 files changed, 51 insertions, 265 deletions

diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
index ad7fc3a269e..27e30987549 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
@@ -17,176 +17,58 @@ subject to the following restrictions:
 #include "btHingeConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
-#include "LinearMath/btSimdMinMax.h"
 #include <new>
 
-
 btHingeConstraint::btHingeConstraint():
 m_enableAngularMotor(false)
 {
 }
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
-									 btVector3& axisInA,btVector3& axisInB)
-									 :btTypedConstraint(rbA,rbB),
-									 m_angularOnly(false),
-									 m_enableAngularMotor(false)
-{
-	m_rbAFrame.getOrigin() = pivotInA;
-	
-	// since no frame is given, assume this to be zero angle and just pick rb transform axis
-	btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
-	btScalar projection = rbAxisA1.dot(axisInA);
-	if (projection > SIMD_EPSILON)
-		rbAxisA1 = rbAxisA1*projection - axisInA;
-	 else
-		rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
-	
-	btVector3 rbAxisA2 = rbAxisA1.cross(axisInA);
-
-	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
-									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
-									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
-
-	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
-	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
-	btVector3 rbAxisB2 =  rbAxisB1.cross(axisInB);
-	
-	
-	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() );
-	
-	//start with free
-	m_lowerLimit = btScalar(1e30);
-	m_upperLimit = btScalar(-1e30);
-	m_biasFactor = 0.3f;
-	m_relaxationFactor = 1.0f;
-	m_limitSoftness = 0.9f;
-	m_solveLimit = false;
-
-}
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
-:btTypedConstraint(rbA), m_angularOnly(false), m_enableAngularMotor(false)
-{
-
-	// since no frame is given, assume this to be zero angle and just pick rb transform axis
-	// fixed axis in worldspace
-	btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
-	btScalar projection = rbAxisA1.dot(axisInA);
-	if (projection > SIMD_EPSILON)
-		rbAxisA1 = rbAxisA1*projection - axisInA;
-	else
-		rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
-
-	btVector3 rbAxisA2 = axisInA.cross(rbAxisA1);
-
-	m_rbAFrame.getOrigin() = pivotInA;
-	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
-									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
-									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
-
-
-	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
-
-	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
-	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
-	btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
-
-
-	m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
-	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
-									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
-									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
-	
-	//start with free
-	m_lowerLimit = btScalar(1e30);
-	m_upperLimit = btScalar(-1e30);
-	m_biasFactor = 0.3f;
-	m_relaxationFactor = 1.0f;
-	m_limitSoftness = 0.9f;
-	m_solveLimit = false;
-}
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
-								     const btTransform& rbAFrame, const btTransform& rbBFrame)
-:btTypedConstraint(rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
+								 btVector3& axisInA,btVector3& axisInB)
+:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
+m_axisInA(axisInA),
+m_axisInB(-axisInB),
 m_angularOnly(false),
 m_enableAngularMotor(false)
 {
-	// flip axis
-	m_rbBFrame.getBasis()[2][0] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[2][1] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
-
-	//start with free
-	m_lowerLimit = btScalar(1e30);
-	m_upperLimit = btScalar(-1e30);
-	m_biasFactor = 0.3f;
-	m_relaxationFactor = 1.0f;
-	m_limitSoftness = 0.9f;
-	m_solveLimit = false;
-}			
 
+}
 
 
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
-:btTypedConstraint(rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
+:btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
+m_axisInA(axisInA),
+//fixed axis in worldspace
+m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA),
 m_angularOnly(false),
 m_enableAngularMotor(false)
 {
-	// flip axis
-	m_rbBFrame.getBasis()[2][0] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[2][1] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
-
-
-	//start with free
-	m_lowerLimit = btScalar(1e30);
-	m_upperLimit = btScalar(-1e30);	
-	m_biasFactor = 0.3f;
-	m_relaxationFactor = 1.0f;
-	m_limitSoftness = 0.9f;
-	m_solveLimit = false;
+	
 }
 
 void	btHingeConstraint::buildJacobian()
 {
 	m_appliedImpulse = btScalar(0.);
 
+	btVector3	normal(0,0,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++)
 		{
+			normal[i] = 1;
 			new (&m_jac[i]) btJacobianEntry(
 				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
 				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-				pivotAInW - m_rbA.getCenterOfMassPosition(),
-				pivotBInW - m_rbB.getCenterOfMassPosition(),
-				normal[i],
+				m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
+				m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
+				normal,
 				m_rbA.getInvInertiaDiagLocal(),
 				m_rbA.getInvMass(),
 				m_rbB.getInvInertiaDiagLocal(),
 				m_rbB.getInvMass());
+			normal[i] = 0;
 		}
 	}
 
@@ -197,12 +79,12 @@ void	btHingeConstraint::buildJacobian()
 	btVector3 jointAxis0local;
 	btVector3 jointAxis1local;
 	
-	btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
+	btPlaneSpace1(m_axisInA,jointAxis0local,jointAxis1local);
 
-	getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+	getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
 	btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
 	btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
-	btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+	btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
 		
 	new (&m_jacAng[0])	btJacobianEntry(jointAxis0,
 		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
@@ -223,71 +105,44 @@ void	btHingeConstraint::buildJacobian()
 		m_rbB.getInvInertiaDiagLocal());
 
 
-	// Compute limit information
-	btScalar hingeAngle = getHingeAngle();  
-
-	//set bias, sign, clear accumulator
-	m_correction = btScalar(0.);
-	m_limitSign = btScalar(0.);
-	m_solveLimit = false;
-	m_accLimitImpulse = btScalar(0.);
-
-	if (m_lowerLimit < m_upperLimit)
-	{
-		if (hingeAngle <= m_lowerLimit*m_limitSoftness)
-		{
-			m_correction = (m_lowerLimit - hingeAngle);
-			m_limitSign = 1.0f;
-			m_solveLimit = true;
-		} 
-		else if (hingeAngle >= m_upperLimit*m_limitSoftness)
-		{
-			m_correction = m_upperLimit - hingeAngle;
-			m_limitSign = -1.0f;
-			m_solveLimit = true;
-		}
-	}
-
-	//Compute K = J*W*J' for hinge axis
-	btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
-	m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
-			             getRigidBodyB().computeAngularImpulseDenominator(axisA));
 
 }
 
 void	btHingeConstraint::solveConstraint(btScalar	timeStep)
 {
 
-	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
+	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
 
+	btVector3 normal(0,0,0);
 	btScalar tau = btScalar(0.3);
 	btScalar damping = btScalar(1.);
 
 //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;
+			normal[i] = 1;
 			btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
 
+			btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+			btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+			
+			btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
+			btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
+			btVector3 vel = vel1 - vel2;
 			btScalar rel_vel;
 			rel_vel = normal.dot(vel);
 			//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;
+			btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  damping * rel_vel * jacDiagABInv * damping;
 			m_appliedImpulse += impulse;
 			btVector3 impulse_vector = normal * impulse;
 			m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
 			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
+			
+			normal[i] = 0;
 		}
 	}
 
@@ -296,8 +151,8 @@ void	btHingeConstraint::solveConstraint(btScalar	timeStep)
 		///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 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
+		btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
 
 		const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
 		const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
@@ -319,7 +174,7 @@ void	btHingeConstraint::solveConstraint(btScalar	timeStep)
 					getRigidBodyB().computeAngularImpulseDenominator(normal);
 				// scale for mass and relaxation
 				//todo:  expose this 0.9 factor to developer
-				velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
+				velrelOrthog *= (btScalar(1.)/denom) * btScalar(0.9);
 			}
 
 			//solve angular positional correction
@@ -335,28 +190,10 @@ void	btHingeConstraint::solveConstraint(btScalar	timeStep)
 
 			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, 0.0f );
-				impulseMag = m_accLimitImpulse - temp;
-
-
-				btVector3 impulse = axisA * impulseMag * m_limitSign;
-				m_rbA.applyTorqueImpulse(impulse);
-				m_rbB.applyTorqueImpulse(-impulse);
-			}
 		}
 
 		//apply motor
-		if (m_enableAngularMotor) 
+		if (m_enableAngularMotor)
 		{
 			//todo: add limits too
 			btVector3 angularLimit(0,0,0);
@@ -367,7 +204,10 @@ void	btHingeConstraint::solveConstraint(btScalar	timeStep)
 			btScalar desiredMotorVel = m_motorTargetVelocity;
 			btScalar motor_relvel = desiredMotorVel - projRelVel;
 
-			btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
+			btScalar denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) +
+					getRigidBodyB().computeAngularImpulseDenominator(axisA);
+
+			btScalar unclippedMotorImpulse = (btScalar(1.)/denom3) * motor_relvel;;
 			//todo: should clip against accumulated impulse
 			btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
 			clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
@@ -387,11 +227,3 @@ void	btHingeConstraint::updateRHS(btScalar	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)  );
-}
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
index 4b42d3d2acd..5c1ceafbc5b 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
@@ -13,8 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */
-
 #ifndef HINGECONSTRAINT_H
 #define HINGECONSTRAINT_H
 
@@ -24,6 +22,7 @@ subject to the following restrictions:
 
 class btRigidBody;
 
+
 /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
 /// axis defines the orientation of the hinge axis
 class btHingeConstraint : public btTypedConstraint
@@ -31,40 +30,22 @@ class btHingeConstraint : public btTypedConstraint
 	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
 	btJacobianEntry	m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
 
-	btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
-	btTransform m_rbBFrame;
-
-	btScalar	m_motorTargetVelocity;
-	btScalar	m_maxMotorImpulse;
-
-	btScalar	m_limitSoftness; 
-	btScalar	m_biasFactor; 
-	btScalar    m_relaxationFactor; 
-
-	btScalar    m_lowerLimit;	
-	btScalar    m_upperLimit;	
-	
-	btScalar	m_kHinge;
-
-	btScalar	m_limitSign;
-	btScalar	m_correction;
-
-	btScalar	m_accLimitImpulse;
+	btVector3	m_pivotInA;
+	btVector3	m_pivotInB;
+	btVector3	m_axisInA;
+	btVector3	m_axisInB;
 
 	bool		m_angularOnly;
-	bool		m_enableAngularMotor;
-	bool		m_solveLimit;
 
+	btScalar		m_motorTargetVelocity;
+	btScalar		m_maxMotorImpulse;
+	bool		m_enableAngularMotor;
 	
 public:
 
-	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB);
+	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,btVector3& axisInA,btVector3& axisInB);
 
 	btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA);
-	
-	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
-
-	btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
 
 	btHingeConstraint();
 
@@ -95,33 +76,6 @@ public:
 		m_maxMotorImpulse = maxMotorImpulse;
 	}
 
-	void	setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
-	{
-		m_lowerLimit = low;
-		m_upperLimit = high;
-
-		m_limitSoftness =  _softness;
-		m_biasFactor = _biasFactor;
-		m_relaxationFactor = _relaxationFactor;
-
-	}
-
-	btScalar getHingeAngle();
-
-
-	const btTransform& getAFrame() { return m_rbAFrame; };	
-	const btTransform& getBFrame() { return m_rbBFrame; };
-
-	inline int getSolveLimit()
-	{
-		return m_solveLimit;
-	}
-
-	inline btScalar getLimitSign()
-	{
-		return m_limitSign;
-	}
-		
 };
 
 #endif //HINGECONSTRAINT_H