Some fixes to work with constraint angle limits close to -PI or PI

Demos/ConstraintDemo/ConstraintDemo.cpp

@@ -169,10 +169,12 @@ void	ConstraintDemo::initPhysics()
 		slider->setLinearUpperLimit(hiSliderLimit);
 
 		//range should be small, otherwise singularities will 'explode' the constraint
-		slider->setAngularLowerLimit(btVector3(-1.5,0,0));
+//		slider->setAngularLowerLimit(btVector3(-1.5,0,0));
-		slider->setAngularUpperLimit(btVector3(1.5,0,0));
+//		slider->setAngularUpperLimit(btVector3(1.5,0,0));
 //		slider->setAngularLowerLimit(btVector3(0,0,0));
 //		slider->setAngularUpperLimit(btVector3(0,0,0));
+		slider->setAngularLowerLimit(btVector3(-SIMD_PI,0,0));
+		slider->setAngularUpperLimit(btVector3(1.5,0,0));
 
 		slider->getTranslationalLimitMotor()->m_enableMotor[0] = true;
 		slider->getTranslationalLimitMotor()->m_targetVelocity[0] = -5.0f;
@@ -198,7 +200,11 @@ void	ConstraintDemo::initPhysics()
 
 		spDoorHinge = new btHingeConstraint( *pDoorBody, btPivotA, btAxisA );
 
-		spDoorHinge->setLimit( 0.0f, M_PI_2 );
+//		spDoorHinge->setLimit( 0.0f, M_PI_2 );
+		// test problem values
+//		spDoorHinge->setLimit( -M_PI, M_PI*0.8f);
+		spDoorHinge->setLimit( -M_PI*0.8f, M_PI);
+//		spDoorHinge->setLimit( 0.0f, 0.0f );
 		m_dynamicsWorld->addConstraint(spDoorHinge);
 		spDoorHinge->setDbgDrawSize(btScalar(5.f));
 

Demos/SliderConstraintDemo/SliderConstraintDemo.cpp

@@ -213,11 +213,13 @@ void SliderConstraintDemo::initPhysics()
 //	spSlider1->setUpperLinLimit(15.0F);
 //	spSlider1->setLowerLinLimit(-10.0F);
 //	spSlider1->setUpperLinLimit(-10.0F);
+
 	spSlider1->setLowerAngLimit(-SIMD_PI / 3.0F);
 	spSlider1->setUpperAngLimit( SIMD_PI / 3.0F);
 #endif
 
 	m_dynamicsWorld->addConstraint(spSlider1, true);
+	spSlider1->setDbgDrawSize(btScalar(5.f));
 
 	// add kinematic rigid body A2
 	worldPos.setValue(0,2,0);
@@ -251,8 +253,13 @@ void SliderConstraintDemo::initPhysics()
 
 //	spSlider2->setLowerAngLimit(SIMD_PI / 2.0F);
 //	spSlider2->setUpperAngLimit(-SIMD_PI / 2.0F);
-	spSlider2->setLowerAngLimit(-SIMD_PI / 2.0F);
+
-	spSlider2->setUpperAngLimit(SIMD_PI / 2.0F);
+	//	spSlider2->setLowerAngLimit(-SIMD_PI / 2.0F);
+//	spSlider2->setUpperAngLimit(SIMD_PI / 2.0F);
+	spSlider2->setLowerAngLimit(-SIMD_PI);
+	spSlider2->setUpperAngLimit(SIMD_PI *0.8F);
+
+
 //	spSlider2->setLowerAngLimit(-0.01F);
 //	spSlider2->setUpperAngLimit(0.01F);
 
@@ -285,6 +292,7 @@ void SliderConstraintDemo::initPhysics()
 //	spSlider2->setSoftnessLimAng(0.1);
 #endif
 	m_dynamicsWorld->addConstraint(spSlider2, true);
+	spSlider2->setDbgDrawSize(btScalar(5.f));
 
 
 #if 1
@@ -306,6 +314,8 @@ void SliderConstraintDemo::initPhysics()
 	btVector3 pivB(-2.5, 0., 0.);
 	spP2PConst = new btPoint2PointConstraint(*pRbA3, *pRbB3, pivA, pivB);
 	m_dynamicsWorld->addConstraint(spP2PConst, true);
+	spP2PConst->setDbgDrawSize(btScalar(5.f));
+
 }
 #endif
 
@@ -335,6 +345,8 @@ void SliderConstraintDemo::initPhysics()
 	spHingeConst->enableAngularMotor(true, 10.0, 0.19);
 
 	m_dynamicsWorld->addConstraint(spHingeConst, true);
+	spHingeConst->setDbgDrawSize(btScalar(5.f));
+
 #endif
 
 } // SliderConstraintDemo::initPhysics()

Demos/SliderConstraintDemo/main.cpp

@@ -17,6 +17,7 @@ int main(int argc,char** argv)
 
         sliderConstraintDemo->initPhysics();
 		sliderConstraintDemo->getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
+		sliderConstraintDemo->setDebugMode(btIDebugDraw::DBG_DrawConstraints+btIDebugDraw::DBG_DrawConstraintLimits);
        
 
         return glutmain(argc, argv,640,480,"Slider Constraint Demo. http://www.continuousphysics.com/Bullet/phpBB2/", sliderConstraintDemo);

src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp

@@ -112,7 +112,6 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value)
 		m_currentLimit = 0;//Free from violation
 		return 0;
 	}
-
 	if (test_value < m_loLimit)
 	{
 		m_currentLimit = 1;//low limit violation
@@ -420,6 +419,7 @@ void btGeneric6DofConstraint::buildAngularJacobian(
 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
 {
 	btScalar angle = m_calculatedAxisAngleDiff[axis_index];
+	angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
 	m_angularLimits[axis_index].m_currentPosition = angle;
 	//test limits
 	m_angularLimits[axis_index].testLimitValue(angle);

src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

@@ -65,8 +65,8 @@ public:
         m_targetVelocity = 0;
         m_maxMotorForce = 0.1f;
         m_maxLimitForce = 300.0f;
-        m_loLimit = -SIMD_INFINITY;
+        m_loLimit = 1.0f;
-        m_hiLimit = SIMD_INFINITY;
+        m_hiLimit = -1.0f;
         m_ERP = 0.5f;
         m_bounce = 0.0f;
         m_damping = 1.0f;
@@ -412,16 +412,14 @@ public:
 
     void	setAngularLowerLimit(const btVector3& angularLower)
     {
-        m_angularLimits[0].m_loLimit = angularLower.getX();
+		for(int i = 0; i < 3; i++) 
-        m_angularLimits[1].m_loLimit = angularLower.getY();
+			m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
-        m_angularLimits[2].m_loLimit = angularLower.getZ();
     }
 
     void	setAngularUpperLimit(const btVector3& angularUpper)
     {
-        m_angularLimits[0].m_hiLimit = angularUpper.getX();
+		for(int i = 0; i < 3; i++)
-        m_angularLimits[1].m_hiLimit = angularUpper.getY();
+			m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
-        m_angularLimits[2].m_hiLimit = angularUpper.getZ();
     }
 
 	//! Retrieves the angular limit informacion
@@ -446,6 +444,8 @@ public:
     	}
     	else
     	{
+			lo = btNormalizeAngle(lo);
+			hi = btNormalizeAngle(hi);
     		m_angularLimits[axis-3].m_loLimit = lo;
     		m_angularLimits[axis-3].m_hiLimit = hi;
     	}

src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

@@ -79,8 +79,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
 									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
 	
 	//start with free
-	m_lowerLimit = btScalar(BT_LARGE_FLOAT);
+	m_lowerLimit = btScalar(1.0f);
-	m_upperLimit = btScalar(-BT_LARGE_FLOAT);
+	m_upperLimit = btScalar(-1.0f);
 	m_biasFactor = 0.3f;
 	m_relaxationFactor = 1.0f;
 	m_limitSoftness = 0.9f;
@@ -119,8 +119,8 @@ m_useReferenceFrameA(useReferenceFrameA)
 									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
 	
 	//start with free
-	m_lowerLimit = btScalar(BT_LARGE_FLOAT);
+	m_lowerLimit = btScalar(1.0f);
-	m_upperLimit = btScalar(-BT_LARGE_FLOAT);
+	m_upperLimit = btScalar(-1.0f);
 	m_biasFactor = 0.3f;
 	m_relaxationFactor = 1.0f;
 	m_limitSoftness = 0.9f;
@@ -139,8 +139,8 @@ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
 m_useReferenceFrameA(useReferenceFrameA)
 {
 	//start with free
-	m_lowerLimit = btScalar(BT_LARGE_FLOAT);
+	m_lowerLimit = btScalar(1.0f);
-	m_upperLimit = btScalar(-BT_LARGE_FLOAT);
+	m_upperLimit = btScalar(-1.0f);
 	m_biasFactor = 0.3f;
 	m_relaxationFactor = 1.0f;
 	m_limitSoftness = 0.9f;
@@ -162,8 +162,8 @@ m_useReferenceFrameA(useReferenceFrameA)
 	m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
 
 	//start with free
-	m_lowerLimit = btScalar(BT_LARGE_FLOAT);
+	m_lowerLimit = btScalar(1.0f);
-	m_upperLimit = btScalar(-BT_LARGE_FLOAT);	
+	m_upperLimit = btScalar(-1.0f);
 	m_biasFactor = 0.3f;
 	m_relaxationFactor = 1.0f;
 	m_limitSoftness = 0.9f;
@@ -648,7 +648,7 @@ btScalar btHingeConstraint::getHingeAngle()
 }
 
 
-
+#if 0
 void btHingeConstraint::testLimit()
 {
 	// Compute limit information
@@ -673,8 +673,36 @@ void btHingeConstraint::testLimit()
 	}
 	return;
 }
+#else
 
 
+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)
+	{
+		m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, 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;
+}
+#endif
+
 static btVector3 vHinge(0, 0, btScalar(1));
 
 void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)

src/BulletDynamics/ConstraintSolver/btHingeConstraint.h

@@ -129,8 +129,8 @@ public:
 
 	void	setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
 	{
-		m_lowerLimit = low;
+		m_lowerLimit = btNormalizeAngle(low);
-		m_upperLimit = high;
+		m_upperLimit = btNormalizeAngle(high);
 
 		m_limitSoftness =  _softness;
 		m_biasFactor = _biasFactor;

src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp

@@ -799,6 +799,7 @@ void btSliderConstraint::testAngLimits(void)
 		const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
 		const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
 		btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));  
+		rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit);
 		m_angPos = rot;
 		if(rot < m_lowerAngLimit)
 		{

src/BulletDynamics/ConstraintSolver/btSliderConstraint.h

@@ -151,9 +151,9 @@ public:
     btScalar getUpperLinLimit() { return m_upperLinLimit; }
     void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
     btScalar getLowerAngLimit() { return m_lowerAngLimit; }
-    void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; }
+    void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
     btScalar getUpperAngLimit() { return m_upperAngLimit; }
-    void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; }
+    void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
 	bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
 	btScalar getSoftnessDirLin() { return m_softnessDirLin; }
 	btScalar getRestitutionDirLin() { return m_restitutionDirLin; }

src/BulletDynamics/ConstraintSolver/btTypedConstraint.h

@@ -244,4 +244,31 @@ public:
 	
 };
 
+// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits 
+// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
+SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
+{
+	if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
+	{
+		return angleInRadians;
+	}
+	else if(angleInRadians < angleLowerLimitInRadians)
+	{
+		btScalar diffLo = btNormalizeAngle(angleLowerLimitInRadians - angleInRadians); // this is positive
+		btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
+		return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
+	}
+	else if(angleInRadians > angleUpperLimitInRadians)
+	{
+		btScalar diffHi = btNormalizeAngle(angleInRadians - angleUpperLimitInRadians); // this is positive
+		btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
+		return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
+	}
+	else
+	{
+		return angleInRadians;
+	}
+}
+
+
 #endif //TYPED_CONSTRAINT_H

src/LinearMath/btScalar.h

@@ -195,6 +195,7 @@ SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y);
 SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); }
 SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
 SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
+SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); }
 
 #else
 		
@@ -232,6 +233,7 @@ SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y)
 SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
 SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
 SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
+SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
 	
 #endif
 
@@ -430,5 +432,22 @@ SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src)
 	return d;
 }
 
+// returns normalized value in range [-SIMD_PI, SIMD_PI]
+SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) 
+{
+	angleInRadians = btFmod(angleInRadians, SIMD_2_PI);
+	if(angleInRadians < -SIMD_PI)
+	{
+		return angleInRadians + SIMD_2_PI;
+	}
+	else if(angleInRadians > SIMD_PI)
+	{
+		return angleInRadians - SIMD_2_PI;
+	}
+	else
+	{
+		return angleInRadians;
+	}
+}
 
 #endif //SIMD___SCALAR_H