improvement for btHingeConstraint to deal with large constraint limits, see Issue 479

Thanks promyclon for the report and patch, and Roman Ponomarev for testing.

src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

@@ -43,6 +43,9 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
 									 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
 									 m_useReferenceFrameA(useReferenceFrameA),
 									 m_flags(0)
+#ifdef _BT_USE_CENTER_LIMIT_
+									,m_limit()
+#endif
 {
 	m_rbAFrame.getOrigin() = pivotInA;
 	
@@ -75,6 +78,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
 									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
 									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
 	
+#ifndef	_BT_USE_CENTER_LIMIT_
 	//start with free
 	m_lowerLimit = btScalar(1.0f);
 	m_upperLimit = btScalar(-1.0f);
@@ -82,6 +86,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
 	m_relaxationFactor = 1.0f;
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
+#endif
 	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }
 
@@ -93,6 +98,9 @@ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
 m_useReferenceFrameA(useReferenceFrameA),
 m_flags(0)
+#ifdef	_BT_USE_CENTER_LIMIT_
+,m_limit()
+#endif
 {
 
 	// since no frame is given, assume this to be zero angle and just pick rb transform axis
@@ -117,6 +125,7 @@ m_flags(0)
 									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
 									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
 	
+#ifndef	_BT_USE_CENTER_LIMIT_
 	//start with free
 	m_lowerLimit = btScalar(1.0f);
 	m_upperLimit = btScalar(-1.0f);
@@ -124,6 +133,7 @@ m_flags(0)
 	m_relaxationFactor = 1.0f;
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
+#endif
 	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }
 
@@ -138,7 +148,11 @@ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
 m_useReferenceFrameA(useReferenceFrameA),
 m_flags(0)
+#ifdef	_BT_USE_CENTER_LIMIT_
+,m_limit()
+#endif
 {
+#ifndef	_BT_USE_CENTER_LIMIT_
 	//start with free
 	m_lowerLimit = btScalar(1.0f);
 	m_upperLimit = btScalar(-1.0f);
@@ -146,6 +160,7 @@ m_flags(0)
 	m_relaxationFactor = 1.0f;
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
+#endif
 	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }			
 
@@ -159,11 +174,14 @@ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
 m_useReferenceFrameA(useReferenceFrameA),
 m_flags(0)
+#ifdef	_BT_USE_CENTER_LIMIT_
+,m_limit()
+#endif
 {
 	///not providing rigidbody B means implicitly using worldspace for body B
 
 	m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
-
+#ifndef	_BT_USE_CENTER_LIMIT_
 	//start with free
 	m_lowerLimit = btScalar(1.0f);
 	m_upperLimit = btScalar(-1.0f);
@@ -171,6 +189,7 @@ m_flags(0)
 	m_relaxationFactor = 1.0f;
 	m_limitSoftness = 0.9f;
 	m_solveLimit = false;
+#endif
 	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
 }
 
@@ -449,8 +468,13 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 	int limit = 0;
 	if(getSolveLimit())
 	{
+#ifdef	_BT_USE_CENTER_LIMIT_
+	limit_err = m_limit.getCorrection() * m_referenceSign;
+#else
 	limit_err = m_correction * m_referenceSign;
+#endif
 	limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+
 	}
 	// if the hinge has joint limits or motor, add in the extra row
 	int powered = 0;
@@ -514,7 +538,11 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 				info->m_upperLimit[srow] = 0;
 			}
 			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
+#ifdef	_BT_USE_CENTER_LIMIT_
+			btScalar bounce = m_limit.getRelaxationFactor();
+#else
 			btScalar bounce = m_relaxationFactor;
+#endif
 			if(bounce > btScalar(0.0))
 			{
 				btScalar vel = angVelA.dot(ax1);
@@ -544,7 +572,11 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
 					}
 				}
 			}
+#ifdef	_BT_USE_CENTER_LIMIT_
+			info->m_constraintError[srow] *= m_limit.getBiasFactor();
+#else
 			info->m_constraintError[srow] *= m_biasFactor;
+#endif
 		} // if(limit)
 	} // if angular limit or powered
 }
@@ -581,38 +613,14 @@ btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTran
 }
 
 
-#if 0
-void btHingeConstraint::testLimit()
-{
-	// Compute limit information
-	m_hingeAngle = getHingeAngle();  
-	m_correction = btScalar(0.);
-	m_limitSign = btScalar(0.);
-	m_solveLimit = false;
-	if (m_lowerLimit <= m_upperLimit)
-	{
-		if (m_hingeAngle <= m_lowerLimit)
-		{
-			m_correction = (m_lowerLimit - m_hingeAngle);
-			m_limitSign = 1.0f;
-			m_solveLimit = true;
-		} 
-		else if (m_hingeAngle >= m_upperLimit)
-		{
-			m_correction = m_upperLimit - m_hingeAngle;
-			m_limitSign = -1.0f;
-			m_solveLimit = true;
-		}
-	}
-	return;
-}
-#else
-
 
 void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
 {
 	// Compute limit information
 	m_hingeAngle = getHingeAngle(transA,transB);
+#ifdef	_BT_USE_CENTER_LIMIT_
+	m_limit.test(m_hingeAngle);
+#else
 	m_correction = btScalar(0.);
 	m_limitSign = btScalar(0.);
 	m_solveLimit = false;
@@ -632,9 +640,10 @@ void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& t
 			m_solveLimit = true;
 		}
 	}
+#endif
 	return;
 }
-#endif
+
 
 static btVector3 vHinge(0, 0, btScalar(1));
 
@@ -665,6 +674,9 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
 
 void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
 {
+#ifdef	_BT_USE_CENTER_LIMIT_
+	m_limit.fit(targetAngle);
+#else
 	if (m_lowerLimit < m_upperLimit)
 	{
 		if (targetAngle < m_lowerLimit)
@@ -672,7 +684,7 @@ void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
 		else if (targetAngle > m_upperLimit)
 			targetAngle = m_upperLimit;
 	}
-
+#endif
 	// compute angular velocity
 	btScalar curAngle  = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
 	btScalar dAngle = targetAngle - curAngle;
@@ -843,8 +855,13 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 	int limit = 0;
 	if(getSolveLimit())
 	{
+#ifdef	_BT_USE_CENTER_LIMIT_
+	limit_err = m_limit.getCorrection() * m_referenceSign;
+#else
 	limit_err = m_correction * m_referenceSign;
+#endif
 	limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+
 	}
 	// if the hinge has joint limits or motor, add in the extra row
 	int powered = 0;
@@ -908,7 +925,11 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 				info->m_upperLimit[srow] = 0;
 			}
 			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
+#ifdef	_BT_USE_CENTER_LIMIT_
+			btScalar bounce = m_limit.getRelaxationFactor();
+#else
 			btScalar bounce = m_relaxationFactor;
+#endif
 			if(bounce > btScalar(0.0))
 			{
 				btScalar vel = angVelA.dot(ax1);
@@ -938,7 +959,11 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 					}
 				}
 			}
+#ifdef	_BT_USE_CENTER_LIMIT_
+			info->m_constraintError[srow] *= m_limit.getBiasFactor();
+#else
 			info->m_constraintError[srow] *= m_biasFactor;
+#endif
 		} // if(limit)
 	} // if angular limit or powered
 }

src/BulletDynamics/ConstraintSolver/btHingeConstraint.h

@@ -17,6 +17,8 @@ subject to the following restrictions:
 
 #ifndef HINGECONSTRAINT_H
 #define HINGECONSTRAINT_H
+#define _BT_USE_CENTER_LIMIT_ 1
+
 
 #include "LinearMath/btVector3.h"
 #include "btJacobianEntry.h"
@@ -33,6 +35,7 @@ class btRigidBody;
 #endif //BT_USE_DOUBLE_PRECISION
 
 
+
 enum btHingeFlags
 {
 	BT_HINGE_FLAGS_CFM_STOP = 1,
@@ -57,17 +60,24 @@ public:
 	btScalar	m_motorTargetVelocity;
 	btScalar	m_maxMotorImpulse;
 
+
+#ifdef	_BT_USE_CENTER_LIMIT_
+	btAngularLimit	m_limit;
+#else
+	btScalar	m_lowerLimit;	
+	btScalar	m_upperLimit;	
+	btScalar	m_limitSign;
+	btScalar	m_correction;
+
 	btScalar	m_limitSoftness; 
 	btScalar	m_biasFactor; 
 	btScalar	m_relaxationFactor; 
 
-	btScalar    m_lowerLimit;	
+	bool		m_solveLimit;
-	btScalar    m_upperLimit;	
+#endif
 
 	btScalar	m_kHinge;
 
-	btScalar	m_limitSign;
-	btScalar	m_correction;
 
 	btScalar	m_accLimitImpulse;
 	btScalar	m_hingeAngle;
@@ -75,7 +85,6 @@ public:
 
 	bool		m_angularOnly;
 	bool		m_enableAngularMotor;
-	bool		m_solveLimit;
 	bool		m_useSolveConstraintObsolete;
 	bool		m_useOffsetForConstraintFrame;
 	bool		m_useReferenceFrameA;
@@ -169,13 +178,15 @@ public:
 
 	void	setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
 	{
+#ifdef	_BT_USE_CENTER_LIMIT_
+		m_limit.set(low, high, _softness, _biasFactor, _relaxationFactor);
+#else
 		m_lowerLimit = btNormalizeAngle(low);
 		m_upperLimit = btNormalizeAngle(high);
-
 		m_limitSoftness =  _softness;
 		m_biasFactor = _biasFactor;
 		m_relaxationFactor = _relaxationFactor;
-
+#endif
 	}
 
 	void	setAxis(btVector3& axisInA)
@@ -202,12 +213,20 @@ public:
 
 	btScalar	getLowerLimit() const
 	{
+#ifdef	_BT_USE_CENTER_LIMIT_
+	return m_limit.getLow();
+#else
 	return m_lowerLimit;
+#endif
 	}
 
 	btScalar	getUpperLimit() const
 	{
+#ifdef	_BT_USE_CENTER_LIMIT_
+	return m_limit.getHigh();
+#else		
 	return m_upperLimit;
+#endif
 	}
 
 
@@ -226,12 +245,20 @@ public:
 
 	inline int getSolveLimit()
 	{
+#ifdef	_BT_USE_CENTER_LIMIT_
+	return m_limit.isLimit();
+#else
 	return m_solveLimit;
+#endif
 	}
 
 	inline btScalar getLimitSign()
 	{
+#ifdef	_BT_USE_CENTER_LIMIT_
+	return m_limit.getSign();
+#else
 		return m_limitSign;
+#endif
 	}
 
 	inline bool getAngularOnly() 
@@ -330,12 +357,19 @@ SIMD_FORCE_INLINE	const char*	btHingeConstraint::serialize(void* dataBuffer, btS
 	hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
 	hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
 	hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
-	
+#ifdef	_BT_USE_CENTER_LIMIT_
+	hingeData->m_lowerLimit = float(m_limit.getLow());
+	hingeData->m_upperLimit = float(m_limit.getHigh());
+	hingeData->m_limitSoftness = float(m_limit.getSoftness());
+	hingeData->m_biasFactor = float(m_limit.getBiasFactor());
+	hingeData->m_relaxationFactor = float(m_limit.getRelaxationFactor());
+#else
 	hingeData->m_lowerLimit = float(m_lowerLimit);
 	hingeData->m_upperLimit = float(m_upperLimit);
 	hingeData->m_limitSoftness = float(m_limitSoftness);
 	hingeData->m_biasFactor = float(m_biasFactor);
 	hingeData->m_relaxationFactor = float(m_relaxationFactor);
+#endif
 
 	return btHingeConstraintDataName;
 }

src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp

@@ -140,3 +140,71 @@ btRigidBody& btTypedConstraint::getFixedBody()
 	return s_fixed;
 }
 
+
+void btAngularLimit::set(btScalar low, btScalar high, btScalar _softness, btScalar _biasFactor, btScalar _relaxationFactor)
+{
+	m_halfRange = (high - low) / 2.0f;
+	m_center = btNormalizeAngle(low + m_halfRange);
+	m_softness =  _softness;
+	m_biasFactor = _biasFactor;
+	m_relaxationFactor = _relaxationFactor;
+}
+
+void btAngularLimit::test(const btScalar angle)
+{
+	m_correction = 0.0f;
+	m_sign = 0.0f;
+	m_solveLimit = false;
+
+	if (m_halfRange >= 0.0f)
+	{
+		btScalar deviation = btNormalizeAngle(angle - m_center);
+		if (deviation < -m_halfRange)
+		{
+			m_solveLimit = true;
+			m_correction = - (deviation + m_halfRange);
+			m_sign = +1.0f;
+		}
+		else if (deviation > m_halfRange)
+		{
+			m_solveLimit = true;
+			m_correction = m_halfRange - deviation;
+			m_sign = -1.0f;
+		}
+	}
+}
+
+
+btScalar btAngularLimit::getError() const
+{
+	return m_correction * m_sign;
+}
+
+void btAngularLimit::fit(btScalar& angle) const
+{
+	if (m_halfRange > 0.0f)
+	{
+		btScalar relativeAngle = btNormalizeAngle(angle - m_center);
+		if (!btEqual(relativeAngle, m_halfRange))
+		{
+			if (relativeAngle > 0.0f)
+			{
+				angle = getHigh();
+			}
+			else
+			{
+				angle = getLow();
+			}
+		}
+	}
+}
+
+btScalar btAngularLimit::getLow() const
+{
+	return btNormalizeAngle(m_center - m_halfRange);
+}
+
+btScalar btAngularLimit::getHigh() const
+{
+	return btNormalizeAngle(m_center + m_halfRange);
+}

src/BulletDynamics/ConstraintSolver/btTypedConstraint.h

@@ -313,5 +313,98 @@ SIMD_FORCE_INLINE	int	btTypedConstraint::calculateSerializeBufferSize() const
 
 
 
+class btAngularLimit
+{
+private:
+	btScalar 
+		m_center,
+		m_halfRange,
+		m_softness,
+		m_biasFactor,
+		m_relaxationFactor,
+		m_correction,
+		m_sign;
+
+	bool
+		m_solveLimit;
+
+public:
+	/// Default constructor initializes limit as inactive, allowing free constraint movement
+	btAngularLimit()
+		:m_center(0.0f),
+		m_halfRange(-1.0f),
+		m_softness(0.9f),
+		m_biasFactor(0.3f),
+		m_relaxationFactor(1.0f),
+		m_correction(0.0f),
+		m_sign(0.0f),
+		m_solveLimit(false)
+	{}
+
+	/// Sets all limit's parameters.
+	/// When low > high limit becomes inactive.
+	/// When high - low > 2PI limit is ineffective too becouse no angle can exceed the limit
+	void set(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f);
+
+	/// Checks conastaint angle against limit. If limit is active and the angle violates the limit
+	/// correction is calculated.
+	void test(const btScalar angle);
+
+	/// Returns limit's softness
+	inline btScalar getSoftness() const
+	{
+		return m_softness;
+	}
+
+	/// Returns limit's bias factor
+	inline btScalar getBiasFactor() const
+	{
+		return m_biasFactor;
+	}
+
+	/// Returns limit's relaxation factor
+	inline btScalar getRelaxationFactor() const
+	{
+		return m_relaxationFactor;
+	}
+
+	/// Returns correction value evaluated when test() was invoked 
+	inline btScalar getCorrection() const
+	{
+		return m_correction;
+	}
+
+	/// Returns sign value evaluated when test() was invoked 
+	inline btScalar getSign() const
+	{
+		return m_sign;
+	}
+
+	/// Gives half of the distance between min and max limit angle
+	inline btScalar getHalfRange() const
+	{
+		return m_halfRange;
+	}
+
+	/// Returns true when the last test() invocation recognized limit violation
+	inline bool isLimit() const
+	{
+		return m_solveLimit;
+	}
+
+	/// Checks given angle against limit. If limit is active and angle doesn't fit it, the angle
+	/// returned is modified so it equals to the limit closest to given angle.
+	void fit(btScalar& angle) const;
+
+	/// Returns correction value multiplied by sign value
+	btScalar getError() const;
+
+	inline btScalar getLow() const;
+
+	inline btScalar getHigh() const;
+
+};
+
+
 
 #endif //TYPED_CONSTRAINT_H