More reliable way of calculating which hinge limit to enforce (#833)

When mTheta == mLimitsMin, GetSmallestAngleToLimit() returns 0 which means GetSmallestAngleToLimit() < 0 is false and the limit would be applied in the wrong direction, making it harder to push the hinge away from its limit. Now we explicitly calculate the closest limit.

See godot-jolt/godot-jolt#733

Jolt/Physics/Constraints/HingeConstraint.cpp

@@ -235,6 +235,13 @@ float HingeConstraint::GetSmallestAngleToLimit() const
 	return abs(dist_to_min) < abs(dist_to_max)? dist_to_min : dist_to_max;
 }
 
+bool HingeConstraint::IsMinLimitClosest() const
+{
+	float dist_to_min = CenterAngleAroundZero(mTheta - mLimitsMin);
+	float dist_to_max = CenterAngleAroundZero(mTheta - mLimitsMax);
+	return abs(dist_to_min) < abs(dist_to_max);
+}
+
 bool HingeConstraint::SolveVelocityConstraint(float inDeltaTime)
 {
 	// Solve motor
@@ -273,7 +280,7 @@ bool HingeConstraint::SolveVelocityConstraint(float inDeltaTime)
 			min_lambda = -FLT_MAX;
 			max_lambda = FLT_MAX;
 		}
-		else if (GetSmallestAngleToLimit() < 0.0f)
+		else if (IsMinLimitClosest())
 		{
 			min_lambda = 0.0f;
 			max_lambda = FLT_MAX;

Jolt/Physics/Constraints/HingeConstraint.h

@@ -127,6 +127,7 @@ private:
 	void						CalculateRotationLimitsConstraintProperties(float inDeltaTime);
 	void						CalculateMotorConstraintProperties(float inDeltaTime);
 	inline float				GetSmallestAngleToLimit() const;
+	inline bool					IsMinLimitClosest() const;
 
 	// CONFIGURATION PROPERTIES FOLLOW