added ConstraintSolver/btConeTwistConstraint.cpp to allow for ragdolls

improved hinge constraint: adds limits
added btAtan2Fast
quaternion helper functions
All thanks to Starbreeze Studios / Marcus Hennix, Marten Svanfeldt

src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp

@@ -0,0 +1,286 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+Written by: Marcus Hennix
+*/
+
+
+#include "btConeTwistConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btSimdMinMax.h"
+#include <new>
+
+btConeTwistConstraint::btConeTwistConstraint()
+{
+}
+
+
+btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, 
+											 const btTransform& rbAFrame,const btTransform& rbBFrame)
+											 :btTypedConstraint(rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
+											 m_angularOnly(false)
+{
+	// flip axis for correct angles
+	m_rbBFrame.getBasis()[1][0] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[1][1] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
+
+	m_swingSpan1 = btScalar(1e30);
+	m_swingSpan2 = btScalar(1e30);
+	m_twistSpan  = btScalar(1e30);
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+
+	m_solveTwistLimit = false;
+	m_solveSwingLimit = false;
+
+}
+
+btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
+											:btTypedConstraint(rbA),m_rbAFrame(rbAFrame),
+											 m_angularOnly(false)
+{
+	m_rbBFrame = m_rbAFrame;
+	
+	// flip axis for correct angles
+	m_rbBFrame.getBasis()[1][0] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[1][1] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
+
+	m_rbBFrame.getBasis()[2][0] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[2][1] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
+	
+	m_swingSpan1 = btScalar(1e30);
+	m_swingSpan2 = btScalar(1e30);
+	m_twistSpan  = btScalar(1e30);
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+
+	m_solveTwistLimit = false;
+	m_solveSwingLimit = false;
+	
+}			
+
+void	btConeTwistConstraint::buildJacobian()
+{
+	m_appliedImpulse = btScalar(0.);
+
+	//set bias, sign, clear accumulator
+	m_swingCorrection = btScalar(0.);
+	m_twistLimitSign = btScalar(0.);
+	m_solveTwistLimit = false;
+	m_solveSwingLimit = false;
+	m_accTwistLimitImpulse = btScalar(0.);
+	m_accSwingLimitImpulse = btScalar(0.);
+
+	if (!m_angularOnly)
+	{
+		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+		btVector3 relPos = pivotBInW - pivotAInW;
+
+		btVector3 normal[3];
+		if (relPos.length2() > SIMD_EPSILON)
+		{
+			normal[0] = relPos.normalized();
+		}
+		else
+		{
+			normal[0].setValue(btScalar(1.0),0,0);
+		}
+
+		btPlaneSpace1(normal[0], normal[1], normal[2]);
+
+		for (int i=0;i<3;i++)
+		{
+			new (&m_jac[i]) btJacobianEntry(
+				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+				pivotAInW - m_rbA.getCenterOfMassPosition(),
+				pivotBInW - m_rbB.getCenterOfMassPosition(),
+				normal[i],
+				m_rbA.getInvInertiaDiagLocal(),
+				m_rbA.getInvMass(),
+				m_rbB.getInvInertiaDiagLocal(),
+				m_rbB.getInvMass());
+		}
+	}
+
+	btVector3 b1Axis1,b1Axis2,b1Axis3;
+	btVector3 b2Axis1,b2Axis2;
+
+	b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
+	b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
+
+	btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
+
+	// Get Frame into world space
+	if (m_swingSpan1 >= btScalar(0.05f))
+	{
+		b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
+		swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
+	}
+
+	if (m_swingSpan2 >= btScalar(0.05f))
+	{
+		b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);			
+		swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
+	}
+
+	btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);		
+	btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);	
+	btScalar EllipseAngle = btFabs(swing1)* RMaxAngle1Sq + btFabs(swing2) * RMaxAngle2Sq;
+
+	if (EllipseAngle > 1.0f)
+	{
+		m_swingCorrection = EllipseAngle-1.0f;
+		m_solveSwingLimit = true;
+		
+		// Calculate necessary axis & factors
+		m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
+		m_swingAxis.normalize();
+
+		btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
+		m_swingAxis *= swingAxisSign;
+
+		m_kSwing =  btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
+			getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
+
+	}
+
+	// Twist limits
+	if (m_twistSpan >= btScalar(0.))
+	{
+		btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
+		btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
+		btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 
+		btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
+
+		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
+		if (twist <= -m_twistSpan*lockedFreeFactor)
+		{
+			m_twistCorrection = -(twist + m_twistSpan);
+			m_solveTwistLimit = true;
+
+			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
+			m_twistAxis.normalize();
+			m_twistAxis *= -1.0f;
+
+			m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
+				getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
+
+		}	else
+			if (twist >  m_twistSpan*lockedFreeFactor)
+			{
+				m_twistCorrection = (twist - m_twistSpan);
+				m_solveTwistLimit = true;
+
+				m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
+				m_twistAxis.normalize();
+
+				m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
+					getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
+
+			}
+	}
+}
+
+void	btConeTwistConstraint::solveConstraint(btScalar	timeStep)
+{
+
+	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+
+	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;
+			btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+
+			btScalar rel_vel;
+			rel_vel = normal.dot(vel);
+			//positional error (zeroth order error)
+			btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+			btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
+			m_appliedImpulse += impulse;
+			btVector3 impulse_vector = normal * impulse;
+			m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
+			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
+		}
+	}
+	
+	{
+		///solve angular part
+		const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
+		const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
+
+		// solve swing limit
+		if (m_solveSwingLimit)
+		{
+			btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
+			btScalar impulseMag = amplitude * m_kSwing;
+
+			// Clamp the accumulated impulse
+			btScalar temp = m_accSwingLimitImpulse;
+			m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, 0.0f );
+			impulseMag = m_accSwingLimitImpulse - temp;
+
+			btVector3 impulse = m_swingAxis * impulseMag;
+
+			m_rbA.applyTorqueImpulse(impulse);
+			m_rbB.applyTorqueImpulse(-impulse);
+
+		}
+
+		// solve twist limit
+		if (m_solveTwistLimit)
+		{
+			btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor );
+			btScalar impulseMag = amplitude * m_kTwist;
+
+			// Clamp the accumulated impulse
+			btScalar temp = m_accTwistLimitImpulse;
+			m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, 0.0f );
+			impulseMag = m_accTwistLimitImpulse - temp;
+
+			btVector3 impulse = m_twistAxis * impulseMag;
+
+			m_rbA.applyTorqueImpulse(impulse);
+			m_rbB.applyTorqueImpulse(-impulse);
+
+		}
+	
+	}
+
+}
+
+void	btConeTwistConstraint::updateRHS(btScalar	timeStep)
+{
+	(void)timeStep;
+
+}

src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

@@ -0,0 +1,123 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+Written by: Marcus Hennix
+*/
+
+
+
+#ifndef CONETWISTCONSTRAINT_H
+#define CONETWISTCONSTRAINT_H
+
+#include "../../LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+class btRigidBody;
+
+
+///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
+class btConeTwistConstraint : public btTypedConstraint
+{
+	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
+
+	btTransform m_rbAFrame; 
+	btTransform m_rbBFrame;
+
+	btScalar	m_limitSoftness;
+	btScalar	m_biasFactor;
+	btScalar	m_relaxationFactor;
+
+	btScalar	m_swingSpan1;
+	btScalar	m_swingSpan2;
+	btScalar	m_twistSpan;
+
+	btVector3   m_swingAxis;
+	btVector3	m_twistAxis;
+
+	btScalar	m_kSwing;
+	btScalar	m_kTwist;
+
+	btScalar	m_twistLimitSign;
+	btScalar	m_swingCorrection;
+	btScalar	m_twistCorrection;
+
+	btScalar	m_accSwingLimitImpulse;
+	btScalar	m_accTwistLimitImpulse;
+
+	bool		m_angularOnly;
+	bool		m_solveTwistLimit;
+	bool		m_solveSwingLimit;
+
+	
+public:
+
+	btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame);
+	
+	btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
+
+	btConeTwistConstraint();
+
+	virtual void	buildJacobian();
+
+	virtual	void	solveConstraint(btScalar	timeStep);
+
+	void	updateRHS(btScalar	timeStep);
+
+	const btRigidBody& getRigidBodyA() const
+	{
+		return m_rbA;
+	}
+	const btRigidBody& getRigidBodyB() const
+	{
+		return m_rbB;
+	}
+
+	void	setAngularOnly(bool angularOnly)
+	{
+		m_angularOnly = angularOnly;
+	}
+
+	void	setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 0.8f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+	{
+		m_swingSpan1 = _swingSpan1;
+		m_swingSpan2 = _swingSpan2;
+		m_twistSpan  = _twistSpan;
+
+		m_limitSoftness =  _softness;
+		m_biasFactor = _biasFactor;
+		m_relaxationFactor = _relaxationFactor;
+	}
+
+	const btTransform& getAFrame() { return m_rbAFrame; };	
+	const btTransform& getBFrame() { return m_rbBFrame; };
+
+	inline int getSolveTwistLimit()
+	{
+		return m_solveTwistLimit;
+	}
+
+	inline int getSolveSwingLimit()
+	{
+		return m_solveTwistLimit;
+	}
+
+	inline btScalar getTwistLimitSign()
+	{
+		return m_twistLimitSign;
+	}
+
+};
+
+#endif //CONETWISTCONSTRAINT_H

src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

@@ -17,58 +17,176 @@ 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)
+									 btVector3& axisInA,btVector3& axisInB)
-:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
+									 :btTypedConstraint(rbA,rbB),
-m_axisInA(axisInA),
+									 m_angularOnly(false),
-m_axisInB(-axisInB),
+									 m_enableAngularMotor(false)
-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_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
+:btTypedConstraint(rbA), m_angularOnly(false), m_enableAngularMotor(false)
-m_axisInA(axisInA),
+{
-//fixed axis in worldspace
+
-m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA),
+	// 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),
 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),
+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(),
-				m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
+				pivotAInW - m_rbA.getCenterOfMassPosition(),
-				m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
+				pivotBInW - m_rbB.getCenterOfMassPosition(),
-				normal,
+				normal[i],
 				m_rbA.getInvInertiaDiagLocal(),
 				m_rbA.getInvMass(),
 				m_rbB.getInvInertiaDiagLocal(),
 				m_rbB.getInvMass());
-			normal[i] = 0;
 		}
 	}
 
@@ -79,12 +197,12 @@ void	btHingeConstraint::buildJacobian()
 	btVector3 jointAxis0local;
 	btVector3 jointAxis1local;
 	
-	btPlaneSpace1(m_axisInA,jointAxis0local,jointAxis1local);
+	btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
 
-	getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
+	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_axisInA;
+	btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
 		
 	new (&m_jacAng[0])	btJacobianEntry(jointAxis0,
 		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
@@ -105,44 +223,71 @@ 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_pivotInA;
+	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
+	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
 
-	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++)
 		{		
-			normal[i] = 1;
+			const btVector3& normal = m_jac[i].m_linearJointAxis;
 			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 -  damping * rel_vel * jacDiagABInv * damping;
+			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());
-			
-			normal[i] = 0;
 		}
 	}
 
@@ -151,8 +296,8 @@ void	btHingeConstraint::solveConstraint(btScalar	timeStep)
 		///solve angular part
 
 		// get axes in world space
-		btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
+		btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
-		btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
+		btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
 
 		const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
 		const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
@@ -174,7 +319,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) * btScalar(0.9);
+				velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
 			}
 
 			//solve angular positional correction
@@ -190,10 +335,28 @@ 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);
@@ -204,10 +367,7 @@ void	btHingeConstraint::solveConstraint(btScalar	timeStep)
 			btScalar desiredMotorVel = m_motorTargetVelocity;
 			btScalar motor_relvel = desiredMotorVel - projRelVel;
 
-			btScalar denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) +
+			btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
-					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;
@@ -227,3 +387,11 @@ 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)  );
+}

src/BulletDynamics/ConstraintSolver/btHingeConstraint.h

@@ -13,6 +13,8 @@ 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
 
@@ -22,7 +24,6 @@ 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
@@ -30,22 +31,40 @@ class btHingeConstraint : public btTypedConstraint
 	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
 	btJacobianEntry	m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
 
-	btVector3	m_pivotInA;
+	btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
-	btVector3	m_pivotInB;
+	btTransform m_rbBFrame;
-	btVector3	m_axisInA;
+
-	btVector3	m_axisInB;
+	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;
 
 	bool		m_angularOnly;
-
-	btScalar		m_motorTargetVelocity;
-	btScalar		m_maxMotorImpulse;
 	bool		m_enableAngularMotor;
+	bool		m_solveLimit;
+
 	
 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::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();
 
@@ -76,6 +95,33 @@ 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

src/LinearMath/btQuaternion.h

@@ -212,6 +212,7 @@ public:
 
 	SIMD_FORCE_INLINE const btScalar& getW() const { return m_unusedW; }
 
+	
 };
 
 
@@ -283,6 +284,36 @@ slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t)
 	return q1.slerp(q2, t);
 }
 
+SIMD_FORCE_INLINE btVector3 
+quatRotate(btQuaternion& rotation, btVector3& v) 
+{
+	btQuaternion q = rotation * v;
+	q *= rotation.inverse();
+	return btVector3(q.getX(),q.getY(),q.getZ());
+}
+
+SIMD_FORCE_INLINE btQuaternion 
+shortestArcQuat(btVector3& v0,btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
+{
+	btVector3 c = v0.cross(v1);
+	btScalar  d = v0.dot(v1);
+
+	if (d < -1.0 + SIMD_EPSILON)
+		return btQuaternion(0.0f,1.0f,0.0f,0.0f); // just pick any vector
+
+	btScalar  s = btSqrt((1.0f + d) * 2.0f);
+	btScalar rs = 1.0f / s;
+
+	return btQuaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
+}
+
+SIMD_FORCE_INLINE btQuaternion 
+shortestArcQuatNormalize(btVector3& v0,btVector3& v1)
+{
+	v0.normalize();
+	v1.normalize();
+	return shortestArcQuat(v0,v1);
+}
 
 #endif
 

src/LinearMath/btScalar.h

@@ -120,7 +120,6 @@ SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
 	
 #endif
 
-
 #define SIMD_2_PI         btScalar(6.283185307179586232)
 #define SIMD_PI           (SIMD_2_PI * btScalar(0.5))
 #define SIMD_HALF_PI      (SIMD_2_PI * btScalar(0.25))
@@ -135,6 +134,22 @@ SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
 #define SIMD_INFINITY     FLT_MAX
 #endif
 
+SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x) 
+{
+	btScalar coeff_1 = SIMD_PI / 4.0f;
+	btScalar coeff_2 = 3.0f * coeff_1;
+	btScalar abs_y = btFabs(y);
+	btScalar angle;
+	if (x >= 0.0f) {
+		btScalar r = (x - abs_y) / (x + abs_y);
+		angle = coeff_1 - coeff_1 * r;
+	} else {
+		btScalar r = (x + abs_y) / (abs_y - x);
+		angle = coeff_2 - coeff_1 * r;
+	}
+	return (y < 0.0f) ? -angle : angle;
+}
+
 SIMD_FORCE_INLINE bool      btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; }
 
 SIMD_FORCE_INLINE bool	btEqual(btScalar a, btScalar eps) {

src/btBulletDynamicsCommon.h

@@ -25,6 +25,7 @@ subject to the following restrictions:
 
 #include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"