Added RagdollSettings::CalculateConstraintPriorities which calculates constraint priorities that boost the priority of joints towards the root of the ragdoll. (#1783)

Docs/ReleaseNotes.md

@@ -7,6 +7,7 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 ### New functionality
 
 * Added new define `JPH_TRACK_SIMULATION_STATS` which tracks simulation statistics on a per body basis. This can be used to figure out bodies are the most expensive to simulate.
+* Added `RagdollSettings::CalculateConstraintPriorities` which calculates constraint priorities that boost the priority of joints towards the root of the ragdoll.
 
 ### Bug Fixes
 

Jolt/Physics/Ragdoll/Ragdoll.cpp

@@ -190,6 +190,38 @@ bool RagdollSettings::Stabilize()
 	return true;
 }
 
+void RagdollSettings::CalculateConstraintPriorities(uint32 inBasePriority)
+{
+	JPH_ASSERT(inBasePriority + (uint32)mParts.size() > inBasePriority, "Base priority is too high and will cause overflows");
+	JPH_ASSERT(mSkeleton->AreJointsCorrectlyOrdered());
+
+	// Calculate priority for each part. Start with the base priority and increment towards the root
+	Array<uint32> priorities;
+	priorities.resize(mParts.size(), inBasePriority);
+	for (int i = (int)mParts.size() - 1; i >= 0; --i)
+	{
+		uint32 cur_priority = inBasePriority;
+		int j = i;
+		do
+		{
+			priorities[j] = max(priorities[j], cur_priority);
+			cur_priority++;
+
+			j = mSkeleton->GetJoint(j).mParentJointIndex;
+		}
+		while (j != -1);
+	}
+
+	// Copy the priorities to the constraints
+	for (uint i = 0, n = (uint)mParts.size(); i < n; ++i)
+		if (mParts[i].mToParent != nullptr)
+			mParts[i].mToParent->mConstraintPriority = priorities[i];
+
+	// Use the minimum of the priorities of connected bodies for additional constraints
+	for (AdditionalConstraint &constraint : mAdditionalConstraints)
+		constraint.mConstraint->mConstraintPriority = min(priorities[constraint.mBodyIdx[0]], priorities[constraint.mBodyIdx[1]]);
+}
+
 void RagdollSettings::DisableParentChildCollisions(const Mat44 *inJointMatrices, float inMinSeparationDistance)
 {
 	int joint_count = mSkeleton->GetJointCount();

Jolt/Physics/Ragdoll/Ragdoll.h

@@ -27,6 +27,11 @@ public:
 	/// @return True on success, false on failure.
 	bool								Stabilize();
 
+	/// Initializes the constraint priorities so that constraints near the leaves of the ragdoll have a lower priority
+	/// than constraints near the root of the ragdoll.
+	/// @param inBasePriority The lowest priority that will be used in the ragdoll.
+	void								CalculateConstraintPriorities(uint32 inBasePriority = 0);
+
 	/// After the ragdoll has been fully configured, call this function to automatically create and add a GroupFilterTable collision filter to all bodies
 	/// and configure them so that parent and children don't collide.
 	///

Samples/Utils/RagdollLoader.cpp

@@ -112,6 +112,9 @@ RagdollSettings *RagdollLoader::sLoad(const char *inFileName, EMotionType inMoti
 	// Stabilize the constraints of the ragdoll
 	ragdoll->Stabilize();
 
+	// Optional: Calculate constraint priorities to give more priority to the root
+	ragdoll->CalculateConstraintPriorities();
+
 	// Calculate body <-> constraint map
 	ragdoll->CalculateBodyIndexToConstraintIndex();
 	ragdoll->CalculateConstraintIndexToBodyIdxPair();
@@ -296,6 +299,9 @@ RagdollSettings *RagdollLoader::sCreate()
 	// Optional: Stabilize the inertia of the limbs
 	settings->Stabilize();
 
+	// Optional: Calculate constraint priorities to give more priority to the root
+	settings->CalculateConstraintPriorities();
+
 	// Disable parent child collisions so that we don't get collisions between constrained bodies
 	settings->DisableParentChildCollisions();