3 new constraints added : btGeneric6DofSpringConstraint, btUniversalConstraint, btHinge2Constraint

Motors for btConeTwistConstraint added (for obsolete solver only) appConstraintDemo changed to test new constraints Several coding-style fixes
2009-05-21 22:10:13 +00:00
parent 8d9c445b73
commit c680791ce9
38 changed files with 989 additions and 412 deletions
--- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
@@ -22,12 +22,13 @@ Written by: Marcus Hennix
 #include "LinearMath/btMinMax.h"
 #include <new>

-//-----------------------------------------------------------------------------

+
+//#define CONETWIST_USE_OBSOLETE_SOLVER true
 #define CONETWIST_USE_OBSOLETE_SOLVER false
 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f)

-//-----------------------------------------------------------------------------
+

 btConeTwistConstraint::btConeTwistConstraint()
 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
@@ -69,7 +70,7 @@ void btConeTwistConstraint::init()
 }


-//-----------------------------------------------------------------------------
+

 void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
 {
@@ -99,9 +100,9 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
 			info->nub--;
 		}
 	}
-} // btConeTwistConstraint::getInfo1()
+}
 	
-//-----------------------------------------------------------------------------
+

 void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
 {
@@ -230,7 +231,7 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
 	}
 }
 	
-//-----------------------------------------------------------------------------
+

 void	btConeTwistConstraint::buildJacobian()
 {
@@ -239,6 +240,7 @@ void	btConeTwistConstraint::buildJacobian()
 		m_appliedImpulse = btScalar(0.);
 		m_accTwistLimitImpulse = btScalar(0.);
 		m_accSwingLimitImpulse = btScalar(0.);
+		m_accMotorImpulse = btVector3(0.,0.,0.);

 		if (!m_angularOnly)
 		{
@@ -277,7 +279,7 @@ void	btConeTwistConstraint::buildJacobian()
 	}
 }

-//-----------------------------------------------------------------------------
+

 void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
 {
@@ -406,10 +408,10 @@ void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver

 			}
 		}
-		else // no motor: do a little damping
+		else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
 		{
-			const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
-			const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
+			btVector3 angVelA; bodyA.getAngularVelocity(angVelA);
+			btVector3 angVelB; bodyB.getAngularVelocity(angVelB);
 			btVector3 relVel = angVelB - angVelA;
 			if (relVel.length2() > SIMD_EPSILON)
 			{
@@ -490,7 +492,7 @@ void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver

 }

-//-----------------------------------------------------------------------------
+

 void	btConeTwistConstraint::updateRHS(btScalar	timeStep)
 {
@@ -498,7 +500,7 @@ void	btConeTwistConstraint::updateRHS(btScalar	timeStep)

 }

-//-----------------------------------------------------------------------------
+

 void btConeTwistConstraint::calcAngleInfo()
 {
@@ -584,12 +586,12 @@ void btConeTwistConstraint::calcAngleInfo()
 			m_twistAxis.normalize();
 		}
 	}
-} // btConeTwistConstraint::calcAngleInfo()
+}


 static btVector3 vTwist(1,0,0); // twist axis in constraint's space

-//-----------------------------------------------------------------------------
+

 void btConeTwistConstraint::calcAngleInfo2()
 {
@@ -597,13 +599,34 @@ void btConeTwistConstraint::calcAngleInfo2()
 	m_twistLimitSign = btScalar(0.);
 	m_solveTwistLimit = false;
 	m_solveSwingLimit = false;
+	// compute rotation of A wrt B (in constraint space)
+	if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
+	{	// it is assumed that setMotorTarget() was alredy called 
+		// and motor target m_qTarget is within constraint limits
+		// TODO : split rotation to pure swing and pure twist
+		// compute desired transforms in world
+		btTransform trPose(m_qTarget);
+		btTransform trA = getRigidBodyA().getCenterOfMassTransform() * m_rbAFrame;
+		btTransform trB = getRigidBodyB().getCenterOfMassTransform() * m_rbBFrame;
+		btTransform trDeltaAB = trB * trPose * trA.inverse();
+		btQuaternion qDeltaAB = trDeltaAB.getRotation();
+		btVector3 swingAxis = 	btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
+		m_swingAxis = swingAxis;
+		m_swingAxis.normalize();
+		m_swingCorrection = qDeltaAB.getAngle();
+		if(!btFuzzyZero(m_swingCorrection))
+		{
+			m_solveSwingLimit = true;
+		}
+		return;
+	}
+

 	{
 		// compute rotation of A wrt B (in constraint space)
 		btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation();
 		btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation();
 		btQuaternion qAB = qB.inverse() * qA;
-
 		// split rotation into cone and twist
 		// (all this is done from B's perspective. Maybe I should be averaging axes...)
 		btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
@@ -756,7 +779,7 @@ void btConeTwistConstraint::calcAngleInfo2()
 			m_twistAngle = btScalar(0.f);
 		}
 	}
-} // btConeTwistConstraint::calcAngleInfo2()
+}



@@ -982,8 +1005,5 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &
 }


-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------


--- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
@@ -17,6 +17,22 @@ Written by: Marcus Hennix



+/*
+Overview:
+
+btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc).
+It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint".
+It divides the 3 rotational DOFs into swing (movement within a cone) and twist.
+Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape.
+(Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.)
+
+In the contraint's frame of reference:
+twist is along the x-axis,
+and swing 1 and 2 are along the z and y axes respectively.
+*/
+
+
+
 #ifndef CONETWISTCONSTRAINT_H
 #define CONETWISTCONSTRAINT_H

@@ -141,7 +157,18 @@ public:
 		};
 	}

-	void	setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan,  btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+	// setLimit(), a few notes:
+	// _softness:
+	//		0->1, recommend ~0.8->1.
+	//		describes % of limits where movement is free.
+	//		beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached.
+	// _biasFactor:
+	//		0->1?, recommend 0.3 +/-0.3 or so.
+	//		strength with which constraint resists zeroth order (angular, not angular velocity) limit violation.
+	// __relaxationFactor:
+	//		0->1, recommend to stay near 1.
+	//		the lower the value, the less the constraint will fight velocities which violate the angular limits.
+	void	setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
 	{
 		m_swingSpan1 = _swingSpan1;
 		m_swingSpan2 = _swingSpan2;
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
@@ -26,7 +26,7 @@ http://gimpact.sf.net


 #define D6_USE_OBSOLETE_METHOD false
-//-----------------------------------------------------------------------------
+

 btGeneric6DofConstraint::btGeneric6DofConstraint()
 :btTypedConstraint(D6_CONSTRAINT_TYPE),
@@ -35,7 +35,7 @@ m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
 {
 }

-//-----------------------------------------------------------------------------
+

 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
 : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
@@ -46,12 +46,12 @@ m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
 {

 }
-//-----------------------------------------------------------------------------
+


 #define GENERIC_D6_DISABLE_WARMSTARTING 1

-//-----------------------------------------------------------------------------
+

 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
@@ -61,7 +61,7 @@ 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);
@@ -129,7 +129,7 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value)

 }

-//-----------------------------------------------------------------------------
+

 btScalar btRotationalLimitMotor::solveAngularLimits(
 	btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
@@ -249,9 +249,9 @@ int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_valu
 	m_currentLimit[limitIndex] = 0;//Free from violation
 	m_currentLimitError[limitIndex] = btScalar(0.f);
 	return 0;
-} // btTranslationalLimitMotor::testLimitValue()
+}
+

-//-----------------------------------------------------------------------------

 btScalar btTranslationalLimitMotor::solveLinearAxis(
 	btScalar timeStep,
@@ -372,7 +372,7 @@ void btGeneric6DofConstraint::calculateAngleInfo()

 }

-//-----------------------------------------------------------------------------
+

 void btGeneric6DofConstraint::calculateTransforms()
 {
@@ -382,7 +382,7 @@ void btGeneric6DofConstraint::calculateTransforms()
 	calculateAngleInfo();
 }

-//-----------------------------------------------------------------------------
+

 void btGeneric6DofConstraint::buildLinearJacobian(
 	btJacobianEntry & jacLinear,const btVector3 & normalWorld,
@@ -400,7 +400,7 @@ void btGeneric6DofConstraint::buildLinearJacobian(
        m_rbB.getInvMass());
 }

-//-----------------------------------------------------------------------------
+

 void btGeneric6DofConstraint::buildAngularJacobian(
 	btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
@@ -413,7 +413,7 @@ void btGeneric6DofConstraint::buildAngularJacobian(

 }

-//-----------------------------------------------------------------------------
+

 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
 {
@@ -423,7 +423,7 @@ bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
 	return m_angularLimits[axis_index].needApplyTorques();
 }

-//-----------------------------------------------------------------------------
+

 void btGeneric6DofConstraint::buildJacobian()
 {
@@ -483,7 +483,7 @@ void btGeneric6DofConstraint::buildJacobian()
 	}
 }

-//-----------------------------------------------------------------------------
+

 void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
 {
@@ -519,7 +519,7 @@ void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
 	}
 }

-//-----------------------------------------------------------------------------
+

 void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
 {
@@ -528,7 +528,7 @@ void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
 	setAngularLimits(info, row);
 }

-//-----------------------------------------------------------------------------
+

 int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
 {
@@ -559,7 +559,7 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
 	return row;
 }

-//-----------------------------------------------------------------------------
+

 int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset)
 {
@@ -582,7 +582,7 @@ int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_o
 	return row;
 }

-//-----------------------------------------------------------------------------
+

 void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
 {
@@ -643,7 +643,7 @@ void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolv
 	}
 }

-//-----------------------------------------------------------------------------
+

 void	btGeneric6DofConstraint::updateRHS(btScalar	timeStep)
 {
@@ -651,21 +651,21 @@ void	btGeneric6DofConstraint::updateRHS(btScalar	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)
 {
@@ -684,9 +684,9 @@ void btGeneric6DofConstraint::calcAnchorPos(void)
 	const btVector3& pB = m_calculatedTransformB.getOrigin();
 	m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
 	return;
-} // btGeneric6DofConstraint::calcAnchorPos()
+}
+

-//-----------------------------------------------------------------------------

 void btGeneric6DofConstraint::calculateLinearInfo()
 {
@@ -696,9 +696,9 @@ void btGeneric6DofConstraint::calculateLinearInfo()
 	{
 		m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
 	}
-} // btGeneric6DofConstraint::calculateLinearInfo()
+}
+

-//-----------------------------------------------------------------------------

 int btGeneric6DofConstraint::get_limit_motor_info2(
 	btRotationalLimitMotor * limot,
@@ -824,6 +824,6 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
    else return 0;
 }

-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
+
+
+
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
@@ -477,4 +477,5 @@ public:

 };

+
 #endif //GENERIC_6DOF_CONSTRAINT_H
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp
@@ -0,0 +1,134 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. 
+
+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.
+*/
+
+#include "btGeneric6DofSpringConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+
+
+btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA)
+	: btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
+{
+	for(int i = 0; i < 6; i++)
+	{
+		m_springEnabled[i] = false;
+		m_equilibriumPoint[i] = btScalar(0.f);
+		m_springStiffness[i] = btScalar(0.f);
+	}
+}
+
+
+void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
+{
+	btAssert((index >= 0) && (index < 6));
+	m_springEnabled[index] = onOff;
+	if(index < 3)
+	{
+		m_linearLimits.m_enableMotor[index] = onOff;
+	}
+	else
+	{
+		m_angularLimits[index - 3].m_enableMotor = onOff;
+	}
+}
+
+
+
+void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness)
+{
+	btAssert((index >= 0) && (index < 6));
+	m_springStiffness[index] = stiffness;
+}
+
+
+void btGeneric6DofSpringConstraint::setEquilibriumPoint()
+{
+	calculateTransforms();
+	for(int i = 0; i < 3; i++)
+	{
+		m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
+	}
+	for(int i = 0; i < 3; i++)
+	{
+		m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i];
+	}
+}
+
+
+
+void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
+{
+	btAssert((index >= 0) && (index < 6));
+	calculateTransforms();
+	if(index < 3)
+	{
+		m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
+	}
+	else
+	{
+		m_equilibriumPoint[index + 3] = m_calculatedAxisAngleDiff[index];
+	}
+}
+
+
+
+void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
+{
+	// it is assumed that calculateTransforms() have been called before this call
+	int i;
+	btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
+	for(i = 0; i < 3; i++)
+	{
+		if(m_springEnabled[i])
+		{
+			// get current position of constraint
+			btScalar currPos = m_calculatedLinearDiff[i];
+			// calculate difference
+			btScalar delta = currPos - m_equilibriumPoint[i];
+			// spring force is (delta * m_stiffness) according to Hooke's Law
+			btScalar force = delta * m_springStiffness[i];
+			m_linearLimits.m_targetVelocity[i] = force  * info->fps;
+			m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps;
+		}
+	}
+	for(i = 0; i < 3; i++)
+	{
+		if(m_springEnabled[i + 3])
+		{
+			// get current position of constraint
+			btScalar currPos = m_calculatedAxisAngleDiff[i];
+			// calculate difference
+			btScalar delta = currPos - m_equilibriumPoint[i+3];
+			// spring force is (-delta * m_stiffness) according to Hooke's Law
+			btScalar force = -delta * m_springStiffness[i+3];
+			m_angularLimits[i].m_targetVelocity = force  * info->fps;
+			m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
+		}
+	}
+}
+
+
+void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
+{
+	// this will be called by constraint solver at the constraint setup stage
+	// set current motor parameters
+	internalUpdateSprings(info);
+	// do the rest of job for constraint setup
+	btGeneric6DofConstraint::getInfo2(info);
+}
+
+
+
+
--- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h
@@ -0,0 +1,52 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. 
+
+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.
+*/
+
+#ifndef GENERIC_6DOF_SPRING_CONSTRAINT_H
+#define GENERIC_6DOF_SPRING_CONSTRAINT_H
+
+
+#include "LinearMath/btVector3.h"
+#include "btTypedConstraint.h"
+#include "btGeneric6DofConstraint.h"
+
+
+/// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF
+
+/// DOF index used in enableSpring() and setStiffness() means:
+/// 0 : translation X
+/// 1 : translation Y
+/// 2 : translation Z
+/// 3 : rotation X (3rd Euler rotational around new position of X axis, range [-PI+epsilon, PI-epsilon] )
+/// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] )
+/// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] )
+
+class btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
+{
+protected:
+	bool		m_springEnabled[6];
+	btScalar	m_equilibriumPoint[6];
+	btScalar	m_springStiffness[6];
+	void internalUpdateSprings(btConstraintInfo2* info);
+public: 
+    btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
+	void enableSpring(int index, bool onOff);
+	void setStiffness(int index, btScalar stiffness);
+	void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
+	void setEquilibriumPoint(int index);  // set the current constraint position/orientation as an equilibrium point for given DOF
+	virtual void getInfo2 (btConstraintInfo2* info);
+};
+
+#endif // GENERIC_6DOF_SPRING_CONSTRAINT_H
+
--- a/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp
@@ -0,0 +1,65 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. 
+
+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.
+*/
+
+
+
+#include "btHinge2Constraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+
+
+
+// constructor
+// anchor, axis1 and axis2 are in world coordinate system
+// axis1 must be orthogonal to axis2
+btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
+: btGeneric6DofSpringConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
+ m_anchor(anchor),
+ m_axis1(axis1),
+ m_axis2(axis2)
+{
+	// build frame basis
+	// 6DOF constraint uses Euler angles and to define limits
+	// it is assumed that rotational order is :
+	// Z - first, allowed limits are (-PI,PI);
+	// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number 
+	// used to prevent constraint from instability on poles;
+	// new position of X, allowed limits are (-PI,PI);
+	// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
+	// Build the frame in world coordinate system first
+	btVector3 zAxis = axis1.normalize();
+	btVector3 xAxis = axis2.normalize();
+	btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system
+	btTransform frameInW;
+	frameInW.setIdentity();
+	frameInW.getBasis().setValue(	xAxis[0], yAxis[0], zAxis[0],	
+									xAxis[1], yAxis[1], zAxis[1],
+									xAxis[2], yAxis[2], zAxis[2]);
+	frameInW.setOrigin(anchor);
+	// now get constraint frame in local coordinate systems
+	m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
+	m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
+	// sei limits
+	setLinearLowerLimit(btVector3(0.f, 0.f, -1.f));
+	setLinearUpperLimit(btVector3(0.f, 0.f,  1.f));
+	// like front wheels of a car
+	setAngularLowerLimit(btVector3(1.f,  0.f, -SIMD_HALF_PI * 0.5f)); 
+	setAngularUpperLimit(btVector3(-1.f, 0.f,  SIMD_HALF_PI * 0.5f));
+	// enable suspension
+	enableSpring(2, true);
+	setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-)
+	setEquilibriumPoint();
+}
+
--- a/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h
@@ -0,0 +1,58 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. 
+
+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.
+*/
+
+#ifndef HINGE2_CONSTRAINT_H
+#define HINGE2_CONSTRAINT_H
+
+
+
+#include "LinearMath/btVector3.h"
+#include "btTypedConstraint.h"
+#include "btGeneric6DofSpringConstraint.h"
+
+
+
+// Constraint similar to ODE Hinge2 Joint
+// has 3 degrees of frredom:
+// 2 rotational degrees of freedom, similar to Euler rotations around Z (axis 1) and X (axis 2)
+// 1 translational (along axis Z) with suspension spring
+
+class btHinge2Constraint : public btGeneric6DofSpringConstraint
+{
+protected:
+	btVector3	m_anchor;
+	btVector3	m_axis1;
+	btVector3	m_axis2;
+public:
+	// constructor
+	// anchor, axis1 and axis2 are in world coordinate system
+	// axis1 must be orthogonal to axis2
+    btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2);
+	// access
+	const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
+	const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
+	const btVector3& getAxis1() { return m_axis1; }
+	const btVector3& getAxis2() { return m_axis2; }
+	btScalar getAngle1() { return getAngle(2); }
+	btScalar getAngle2() { return getAngle(0); }
+	// limits
+	void setUpperLimit(btScalar ang1max) { setAngularUpperLimit(btVector3(-1.f, 0.f, ang1max)); }
+	void setLowerLimit(btScalar ang1min) { setAngularLowerLimit(btVector3( 1.f, 0.f, ang1min)); }
+};
+
+
+
+#endif // HINGE2_CONSTRAINT_H
+
--- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
@@ -21,11 +21,11 @@ subject to the following restrictions:
 #include <new>
 #include "btSolverBody.h"

-//-----------------------------------------------------------------------------
+

 #define HINGE_USE_OBSOLETE_SOLVER false

-//-----------------------------------------------------------------------------
+


 btHingeConstraint::btHingeConstraint()
@@ -37,7 +37,7 @@ 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, bool useReferenceFrameA)
@@ -88,7 +88,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
 	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), 
@@ -128,7 +128,7 @@ m_useReferenceFrameA(useReferenceFrameA)
 	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }

-//-----------------------------------------------------------------------------
+

 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
 								     const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
@@ -148,7 +148,7 @@ m_useReferenceFrameA(useReferenceFrameA)
 	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }			

-//-----------------------------------------------------------------------------
+

 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
@@ -171,13 +171,14 @@ m_useReferenceFrameA(useReferenceFrameA)
 	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }

-//-----------------------------------------------------------------------------
+

 void	btHingeConstraint::buildJacobian()
 {
 	if (m_useSolveConstraintObsolete)
 	{
 		m_appliedImpulse = btScalar(0.);
+		m_accMotorImpulse = btScalar(0.);

 		if (!m_angularOnly)
 		{
@@ -258,7 +259,7 @@ void	btHingeConstraint::buildJacobian()
 	}
 }

-//-----------------------------------------------------------------------------
+

 void btHingeConstraint::getInfo1(btConstraintInfo1* info)
 {
@@ -279,9 +280,9 @@ void btHingeConstraint::getInfo1(btConstraintInfo1* info)
 			info->nub--; 
 		}
 	}
-} // btHingeConstraint::getInfo1 ()
+}
+

-//-----------------------------------------------------------------------------

 void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
 {
@@ -467,7 +468,7 @@ void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
 	} // if angular limit or powered
 }

-//-----------------------------------------------------------------------------
+

 void	btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
 {
@@ -601,10 +602,22 @@ void	btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody
 				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;
+
+				// accumulated impulse clipping:
+				btScalar fMaxImpulse = m_maxMotorImpulse;
+				btScalar newAccImpulse = m_accMotorImpulse + unclippedMotorImpulse;
+				btScalar clippedMotorImpulse = unclippedMotorImpulse;
+				if (newAccImpulse > fMaxImpulse)
+				{
+					newAccImpulse = fMaxImpulse;
+					clippedMotorImpulse = newAccImpulse - m_accMotorImpulse;
+				}
+				else if (newAccImpulse < -fMaxImpulse)
+				{
+					newAccImpulse = -fMaxImpulse;
+					clippedMotorImpulse = newAccImpulse - m_accMotorImpulse;
+				}
+				m_accMotorImpulse += clippedMotorImpulse;
 			
 				bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
 				bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
@@ -615,7 +628,7 @@ void	btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody

 }

-//-----------------------------------------------------------------------------
+

 void	btHingeConstraint::updateRHS(btScalar	timeStep)
 {
@@ -623,7 +636,7 @@ void	btHingeConstraint::updateRHS(btScalar	timeStep)

 }

-//-----------------------------------------------------------------------------
+

 btScalar btHingeConstraint::getHingeAngle()
 {
@@ -634,7 +647,7 @@ btScalar btHingeConstraint::getHingeAngle()
 	return m_referenceSign * angle;
 }

-//-----------------------------------------------------------------------------
+

 void btHingeConstraint::testLimit()
 {
@@ -659,8 +672,50 @@ void btHingeConstraint::testLimit()
 		}
 	}
 	return;
-} // btHingeConstraint::testLimit()
+}
+
+
+static btVector3 vHinge(0, 0, btScalar(1));
+
+void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
+{
+	// convert target from body to constraint space
+	btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
+	qConstraint.normalize();
+
+	// extract "pure" hinge component
+	btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize();
+	btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
+	btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
+	qHinge.normalize();
+
+	// compute angular target, clamped to limits
+	btScalar targetAngle = qHinge.getAngle();
+	if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
+	{
+		qHinge = operator-(qHinge);
+		targetAngle = qHinge.getAngle();
+	}
+	if (qHinge.getZ() < 0)
+		targetAngle = -targetAngle;
+
+	setMotorTarget(targetAngle, dt);
+}
+
+void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
+{
+	if (m_lowerLimit < m_upperLimit)
+	{
+		if (targetAngle < m_lowerLimit)
+			targetAngle = m_lowerLimit;
+		else if (targetAngle > m_upperLimit)
+			targetAngle = m_upperLimit;
+	}
+
+	// compute angular velocity
+	btScalar curAngle  = getHingeAngle();
+	btScalar dAngle = targetAngle - curAngle;
+	m_motorTargetVelocity = dAngle / dt;
+}
+

-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
--- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
@@ -62,6 +62,8 @@ public:
 	bool		m_useSolveConstraintObsolete;
 	bool		m_useReferenceFrameA;

+	btScalar	m_accMotorImpulse;
+
 	
 public:

@@ -116,6 +118,15 @@ public:
 		m_maxMotorImpulse = maxMotorImpulse;
 	}

+	// extra motor API, including ability to set a target rotation (as opposed to angular velocity)
+	// note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to
+	//       maintain a given angular target.
+	void enableMotor(bool enableMotor) 	{ m_enableAngularMotor = enableMotor; }
+	void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
+	void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
+	void setMotorTarget(btScalar targetAngle, btScalar dt);
+
+
 	void	setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
 	{
 		m_lowerLimit = low;
--- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
@@ -18,14 +18,14 @@ Added by Roman Ponomarev (rponom@gmail.com)
 April 04, 2008
 */

-//-----------------------------------------------------------------------------
+

 #include "btSliderConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
 #include <new>

-//-----------------------------------------------------------------------------
+

 void btSliderConstraint::initParams()
 {
@@ -62,9 +62,9 @@ void btSliderConstraint::initParams()
    m_maxAngMotorForce = btScalar(0.);
 	m_accumulatedAngMotorImpulse = btScalar(0.0);

-} // btSliderConstraint::initParams()
+}
+

-//-----------------------------------------------------------------------------

 btSliderConstraint::btSliderConstraint()
        :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
@@ -73,9 +73,9 @@ btSliderConstraint::btSliderConstraint()
 //		m_useSolveConstraintObsolete(true)
 {
 	initParams();
-} // btSliderConstraint::btSliderConstraint()
+}
+

-//-----------------------------------------------------------------------------

 btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB)
@@ -86,9 +86,9 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const
 //		m_useSolveConstraintObsolete(true)
 {
 	initParams();
-} // btSliderConstraint::btSliderConstraint()
+}
+

-//-----------------------------------------------------------------------------
 static btRigidBody s_fixed(0, 0, 0);
 btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, s_fixed, rbB)
@@ -102,9 +102,9 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& fram
 //	m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());

 	initParams();
-} // btSliderConstraint::btSliderConstraint()
+}
+

-//-----------------------------------------------------------------------------

 void btSliderConstraint::buildJacobian()
 {
@@ -120,9 +120,9 @@ void btSliderConstraint::buildJacobian()
 	{
 		buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA);
 	}
-} // btSliderConstraint::buildJacobian()
+}
+

-//-----------------------------------------------------------------------------

 void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
 {
@@ -175,9 +175,9 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co
 	// clear accumulator for motors
 	m_accumulatedLinMotorImpulse = btScalar(0.0);
 	m_accumulatedAngMotorImpulse = btScalar(0.0);
-} // btSliderConstraint::buildJacobianInt()
+}
+

-//-----------------------------------------------------------------------------

 void btSliderConstraint::getInfo1(btConstraintInfo1* info)
 {
@@ -205,9 +205,9 @@ void btSliderConstraint::getInfo1(btConstraintInfo1* info)
 			info->nub--; 
 		}
 	}
-} // btSliderConstraint::getInfo1()
+}
+

-//-----------------------------------------------------------------------------

 void btSliderConstraint::getInfo2(btConstraintInfo2* info)
 {
@@ -515,9 +515,9 @@ void btSliderConstraint::getInfo2(btConstraintInfo2* info)
 			info->m_constraintError[srow] *= getSoftnessLimAng();
 		} // if(limit)
 	} // if angular limit or powered
-} // btSliderConstraint::getInfo2()
+}
+

-//-----------------------------------------------------------------------------

 void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
 {
@@ -533,9 +533,9 @@ void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBod
 			solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
 		}
 	}
-} // btSliderConstraint::solveConstraint()
+}
+

-//-----------------------------------------------------------------------------

 void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
 {
@@ -719,11 +719,11 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& body
 			bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse);
 		}
 	}
-} // btSliderConstraint::solveConstraint()
+}
+
+

-//-----------------------------------------------------------------------------

-//-----------------------------------------------------------------------------

 void btSliderConstraint::calculateTransforms(void){
 	if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
@@ -756,9 +756,9 @@ void btSliderConstraint::calculateTransforms(void){
 		normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
 		m_depth[i] = m_delta.dot(normalWorld);
    }
-} // btSliderConstraint::calculateTransforms()
+}
 
-//-----------------------------------------------------------------------------
+

 void btSliderConstraint::testLinLimits(void)
 {
@@ -785,9 +785,9 @@ void btSliderConstraint::testLinLimits(void)
 	{
 		m_depth[0] = btScalar(0.);
 	}
-} // btSliderConstraint::testLinLimits()
+}
+

-//-----------------------------------------------------------------------------

 void btSliderConstraint::testAngLimits(void)
 {
@@ -811,9 +811,9 @@ void btSliderConstraint::testAngLimits(void)
 			m_solveAngLim = true;
 		}
 	}
-} // btSliderConstraint::testAngLimits()
+}
 	
-//-----------------------------------------------------------------------------
+

 btVector3 btSliderConstraint::getAncorInA(void)
 {
@@ -821,13 +821,13 @@ btVector3 btSliderConstraint::getAncorInA(void)
 	ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
 	ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
 	return ancorInA;
-} // btSliderConstraint::getAncorInA()
+}
+

-//-----------------------------------------------------------------------------

 btVector3 btSliderConstraint::getAncorInB(void)
 {
 	btVector3 ancorInB;
 	ancorInB = m_frameInB.getOrigin();
 	return ancorInB;
-} // btSliderConstraint::getAncorInB();
+}
--- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
@@ -25,23 +25,23 @@ TODO:
 #ifndef SLIDER_CONSTRAINT_H
 #define SLIDER_CONSTRAINT_H

-//-----------------------------------------------------------------------------
+

 #include "LinearMath/btVector3.h"
 #include "btJacobianEntry.h"
 #include "btTypedConstraint.h"

-//-----------------------------------------------------------------------------
+

 class btRigidBody;

-//-----------------------------------------------------------------------------
+

 #define SLIDER_CONSTRAINT_DEF_SOFTNESS		(btScalar(1.0))
 #define SLIDER_CONSTRAINT_DEF_DAMPING		(btScalar(1.0))
 #define SLIDER_CONSTRAINT_DEF_RESTITUTION	(btScalar(0.7))

-//-----------------------------------------------------------------------------
+

 class btSliderConstraint : public btTypedConstraint
 {
@@ -224,7 +224,7 @@ public:
 	btVector3 getAncorInB(void);
 };

-//-----------------------------------------------------------------------------
+

 #endif //SLIDER_CONSTRAINT_H

--- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
@@ -60,7 +60,7 @@ m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
 }


-//-----------------------------------------------------------------------------
+

 btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
 {
@@ -109,6 +109,6 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal
 			lim_fact = btScalar(0.0f);
 	}
 	return lim_fact;
-} // btTypedConstraint::getMotorFactor()
+}


--- a/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp
@@ -0,0 +1,63 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. 
+
+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.
+*/
+
+
+
+#include "btUniversalConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+
+
+
+#define UNIV_EPS btScalar(0.01f)
+
+
+// constructor
+// anchor, axis1 and axis2 are in world coordinate system
+// axis1 must be orthogonal to axis2
+btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
+: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
+ m_anchor(anchor),
+ m_axis1(axis1),
+ m_axis2(axis2)
+{
+	// build frame basis
+	// 6DOF constraint uses Euler angles and to define limits
+	// it is assumed that rotational order is :
+	// Z - first, allowed limits are (-PI,PI);
+	// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number 
+	// used to prevent constraint from instability on poles;
+	// new position of X, allowed limits are (-PI,PI);
+	// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
+	// Build the frame in world coordinate system first
+	btVector3 zAxis = axis1.normalize();
+	btVector3 yAxis = axis2.normalize();
+	btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
+	btTransform frameInW;
+	frameInW.setIdentity();
+	frameInW.getBasis().setValue(	xAxis[0], yAxis[0], zAxis[0],	
+									xAxis[1], yAxis[1], zAxis[1],
+									xAxis[2], yAxis[2], zAxis[2]);
+	frameInW.setOrigin(anchor);
+	// now get constraint frame in local coordinate systems
+	m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
+	m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
+	// sei limits
+	setLinearLowerLimit(btVector3(0., 0., 0.));
+	setLinearUpperLimit(btVector3(0., 0., 0.));
+	setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS));
+	setAngularUpperLimit(btVector3(0.f,  SIMD_HALF_PI - UNIV_EPS,  SIMD_PI - UNIV_EPS));
+}
+
--- a/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h
@@ -0,0 +1,60 @@
+/*
+Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
+Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. 
+
+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.
+*/
+
+#ifndef UNIVERSAL_CONSTRAINT_H
+#define UNIVERSAL_CONSTRAINT_H
+
+
+
+#include "LinearMath/btVector3.h"
+#include "btTypedConstraint.h"
+#include "btGeneric6DofConstraint.h"
+
+
+
+/// Constraint similar to ODE Universal Joint
+/// has 2 rotatioonal degrees of freedom, similar to Euler rotations around Z (axis 1)
+/// and Y (axis 2)
+/// Description from ODE manual : 
+/// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular. 
+/// In other words, rotation of the two bodies about the direction perpendicular to the two axes will be equal."
+
+class btUniversalConstraint : public btGeneric6DofConstraint
+{
+protected:
+	btVector3	m_anchor;
+	btVector3	m_axis1;
+	btVector3	m_axis2;
+public:
+	// constructor
+	// anchor, axis1 and axis2 are in world coordinate system
+	// axis1 must be orthogonal to axis2
+    btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2);
+	// access
+	const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); }
+	const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); }
+	const btVector3& getAxis1() { return m_axis1; }
+	const btVector3& getAxis2() { return m_axis2; }
+	btScalar getAngle1() { return getAngle(2); }
+	btScalar getAngle2() { return getAngle(1); }
+	// limits
+	void setUpperLimit(btScalar ang1max, btScalar ang2max) { setAngularUpperLimit(btVector3(0.f, ang1max, ang2max)); }
+	void setLowerLimit(btScalar ang1min, btScalar ang2min) { setAngularLowerLimit(btVector3(0.f, ang1min, ang2min)); }
+};
+
+
+
+#endif // UNIVERSAL_CONSTRAINT_H
+