Ability to drive hinge constraints with Ragdoll::DriveToPoseUsingMotors (jrouwe#1613)

Fixes jrouwe#1612

Author: jrouwe

Docs/ReleaseNotes.md

@@ -2,6 +2,19 @@
 
 For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysics/blob/master/Docs/APIChanges.md).
 
+## Unreleased Changes
+
+### New functionality
+
+* Added ability to drive hinge constraints with `Ragdoll::DriveToPoseUsingMotors`. This also adds `HingeConstraint::SetTargetOrientationBS` which sets the target angle in body space.
+* Added `JPH_USE_EXTERNAL_PROFILE` cmake option that allows overriding the behavior of the profile macros.
+
+### Bug Fixes
+
+* Fixed compiling in double precision and fixed issues with floating point contraction that caused unit test failures on LoongArch architecture.
+* Added an epsilon to the `CastRay` / `CastShape` early out condition to avoid dividing by a very small number and overflowing to INF. This can cause a float overflow exception.
+* Fixed Samples requiring Vulkan extension `VK_EXT_device_address_binding_report` without checking if it is available.
+
 ## v5.3.0
 
 ### New functionality

Jolt/Physics/Constraints/HingeConstraint.cpp

@@ -134,6 +134,25 @@ float HingeConstraint::GetCurrentAngle() const
 	return diff.GetRotationAngle(rotation1 * mLocalSpaceHingeAxis1);
 }
 
+void HingeConstraint::SetTargetOrientationBS(QuatArg inOrientation)
+{
+	// See: CalculateA1AndTheta
+	//
+	// The rotation between body 1 and 2 can be written as:
+	//
+	// q2 = q1 rh1 r0
+	//
+	// where rh1 is a rotation around local hinge axis 1, also:
+	//
+	// q2 = q1 inOrientation
+	//
+	// This means:
+	//
+	// rh1 r0 = inOrientation <=> rh1 = inOrientation * r0^-1
+	Quat rh1 = inOrientation * mInvInitialOrientation;
+	SetTargetAngle(rh1.GetRotationAngle(mLocalSpaceHingeAxis1));
+}
+
 void HingeConstraint::SetLimits(float inLimitsMin, float inLimitsMax)
 {
 	JPH_ASSERT(inLimitsMin <= 0.0f && inLimitsMin >= -JPH_PI);

Jolt/Physics/Constraints/HingeConstraint.h

@@ -123,6 +123,11 @@ class JPH_EXPORT HingeConstraint final : public TwoBodyConstraint
 	void						SetTargetAngle(float inAngle)							{ mTargetAngle = mHasLimits? Clamp(inAngle, mLimitsMin, mLimitsMax) : inAngle; } ///< rad
 	float						GetTargetAngle() const									{ return mTargetAngle; }
 
+	/// Set the target orientation in body space (R2 = R1 * inOrientation, where R1 and R2 are the world space rotations for body 1 and 2).
+	/// Calculates the local space target angle and calls SetTargetAngle. Motor state must be EMotorState::Position for this to have any effect.
+	/// May set the wrong angle if inOrientation contains large rotations around other axis than the hinge axis.
+	void						SetTargetOrientationBS(QuatArg inOrientation);
+
 	/// Update the rotation limits of the hinge, value in radians (see HingeConstraintSettings)
 	void						SetLimits(float inLimitsMin, float inLimitsMax);
 	float						GetLimitsMin() const									{ return mLimitsMin; }

Jolt/Physics/Ragdoll/Ragdoll.cpp

@@ -6,6 +6,7 @@
 
 #include <Jolt/Physics/Ragdoll/Ragdoll.h>
 #include <Jolt/Physics/Constraints/SwingTwistConstraint.h>
+#include <Jolt/Physics/Constraints/HingeConstraint.h>
 #include <Jolt/Physics/PhysicsSystem.h>
 #include <Jolt/Physics/Body/BodyLockMulti.h>
 #include <Jolt/Physics/Collision/GroupFilterTable.h>
@@ -635,6 +636,12 @@ void Ragdoll::DriveToPoseUsingMotors(const SkeletonPose &inPose)
 				st_constraint->SetTwistMotorState(EMotorState::Position);
 				st_constraint->SetTargetOrientationBS(joint_state.mRotation);
 			}
+			else if (sub_type == EConstraintSubType::Hinge)
+			{
+				HingeConstraint *h_constraint = static_cast<HingeConstraint *>(constraint);
+				h_constraint->SetMotorState(EMotorState::Position);
+				h_constraint->SetTargetOrientationBS(joint_state.mRotation);
+			}
 			else
 				JPH_ASSERT(false, "Constraint type not implemented!");
 		}

Samples/Utils/RagdollLoader.cpp

@@ -70,6 +70,7 @@ RagdollSettings *RagdollLoader::sLoad(const char *inFileName, EMotionType inMoti
 					settings->mLimitsMin = -original->mNormalHalfConeAngle;
 					settings->mLimitsMax = original->mNormalHalfConeAngle;
 					settings->mMaxFrictionTorque = original->mMaxFrictionTorque;
+					settings->mMotorSettings = original->mSwingMotorSettings;
 					p.mToParent = settings;
 					break;
 				}
@@ -83,6 +84,7 @@ RagdollSettings *RagdollLoader::sLoad(const char *inFileName, EMotionType inMoti
 					settings->mLimitsMin = -1.0f;
 					settings->mLimitsMax = 1.0f;
 					settings->mMaxFrictionForce = original->mMaxFrictionTorque;
+					settings->mMotorSettings = original->mSwingMotorSettings;
 					p.mToParent = settings;
 					break;
 				}