Added functionality to lock the translation of a mapped joint (#312)

This can be used to prevent visual stretching of a ragdoll due to constraint violation by stress.

Jolt/Skeleton/Skeleton.cpp

@@ -36,6 +36,15 @@ void Skeleton::CalculateParentJointIndices()
 		j.mParentJointIndex = GetJointIndex(j.mParentName);
 }
 
+bool Skeleton::AreJointsCorrectlyOrdered() const
+{
+	for (int i = 0; i < (int)mJoints.size(); ++i)
+		if (mJoints[i].mParentJointIndex >= i)
+			return false;
+
+	return true;
+}
+
 void Skeleton::SaveBinaryState(StreamOut &inStream) const
 {
 	inStream.Write((uint32)mJoints.size());

Jolt/Skeleton/Skeleton.h

@@ -53,6 +53,10 @@ public:
 	/// Fill in parent joint indices based on name
 	void					CalculateParentJointIndices();
 
+	/// Many of the algorithms that use the Skeleton class require that parent joints are in the mJoints array before their children.
+	/// This function returns true if this is the case, false if not.
+	bool					AreJointsCorrectlyOrdered() const;
+
 	/// Saves the state of this object in binary form to inStream.
 	void					SaveBinaryState(StreamOut &inStream) const;
 

Jolt/Skeleton/SkeletonMapper.cpp

@@ -102,6 +102,59 @@ void SkeletonMapper::Initialize(const Skeleton *inSkeleton1, const Mat44 *inNeut
 			mUnmapped.emplace_back(j2, inSkeleton2->GetJoint(j2).mParentJointIndex);
 }
 
+void SkeletonMapper::LockTranslations(const Skeleton *inSkeleton2, const bool *inLockedTranslations, const Mat44 *inNeutralPose2)
+{
+	JPH_ASSERT(inSkeleton2->AreJointsCorrectlyOrdered());
+
+	int n = inSkeleton2->GetJointCount();
+
+	// Copy locked joints to array but don't actually include the first joint (this is physics driven)
+	for (int i = 0; i < n; ++i)
+		if (inLockedTranslations[i])
+		{
+			Locked l;
+			l.mJointIdx = i;
+			l.mParentJointIdx = inSkeleton2->GetJoint(i).mParentJointIndex;
+			if (l.mParentJointIdx >= 0)
+				l.mTranslation = inNeutralPose2[l.mParentJointIdx].Inversed() * inNeutralPose2[i].GetTranslation();
+			else
+				l.mTranslation = inNeutralPose2[i].GetTranslation();
+			mLockedTranslations.push_back(l);
+		}
+}
+
+void SkeletonMapper::LockAllTranslations(const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2)
+{
+	JPH_ASSERT(!mMappings.empty(), "Call Initialize first!");
+	JPH_ASSERT(inSkeleton2->AreJointsCorrectlyOrdered());
+
+	// The first mapping is the top most one (remember that joints should be ordered so that parents go before children).
+	// Because we created the mappings from the lowest joint first, this should contain the first mappable joint.
+	int root_idx = mMappings[0].mJointIdx2;
+
+	// Create temp array to hold locked joints
+	int n = inSkeleton2->GetJointCount();
+	bool *locked_translations = (bool *)JPH_STACK_ALLOC(n * sizeof(bool));
+	memset(locked_translations, 0, n * sizeof(bool));
+
+	// Mark root as locked
+	locked_translations[root_idx] = true;
+
+	// Loop over all joints and propagate the locked flag to all children
+	for (int i = root_idx + 1; i < n; ++i)
+	{
+		int parent_idx = inSkeleton2->GetJoint(i).mParentJointIndex;
+		if (parent_idx >= 0)
+			locked_translations[i] = locked_translations[parent_idx];
+	}
+
+	// Unmark root because we don't actually want to include this (this determines the position of the entire ragdoll)
+	locked_translations[root_idx] = false;
+
+	// Call the generic function
+	LockTranslations(inSkeleton2, locked_translations, inNeutralPose2);
+}
+
 void SkeletonMapper::Map(const Mat44 *inPose1ModelSpace, const Mat44 *inPose2LocalSpace, Mat44 *outPose2ModelSpace) const
 {
 	// Apply direct mappings
@@ -143,6 +196,10 @@ void SkeletonMapper::Map(const Mat44 *inPose1ModelSpace, const Mat44 *inPose2Loc
 		}
 		else
 			outPose2ModelSpace[u.mJointIdx] = inPose2LocalSpace[u.mJointIdx];
+
+	// Update all locked joint translations
+	for (const Locked &l : mLockedTranslations)
+		outPose2ModelSpace[l.mJointIdx].SetTranslation(outPose2ModelSpace[l.mParentJointIdx] * l.mTranslation);
 }
 
 void SkeletonMapper::MapReverse(const Mat44 *inPose2ModelSpace, Mat44 *outPose1ModelSpace) const

Jolt/Skeleton/SkeletonMapper.h

@@ -47,6 +47,15 @@ public:
 		int					mParentJointIdx;															///< Parent joint index of unmappable joint
 	};
 
+	/// Joints that should have their translation locked (fixed)
+	class Locked
+	{
+	public:
+		int					mJointIdx;																	///< Joint index of joint with locked translation
+		int					mParentJointIdx;															///< Parent joint index of joint with locked translation
+		Vec3				mTranslation;																///< Translation of neutral pose
+	};
+
 	/// A function that is called to determine if a joint can be mapped from source to target skeleton
 	using CanMapJoint = function<bool (const Skeleton *, int, const Skeleton *, int)>;
 
@@ -66,6 +75,23 @@ public:
 	/// @param inCanMapJoint Function that checks if joints in skeleton 1 and skeleton 2 are equal.
 	void					Initialize(const Skeleton *inSkeleton1, const Mat44 *inNeutralPose1, const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2, const CanMapJoint &inCanMapJoint = sDefaultCanMapJoint);
 
+	/// This can be called so lock the translation of a specified set of joints in skeleton 2.
+	/// Because constraints are never 100% rigid, there's always a little bit of stretch in the ragdoll when the ragdoll is under stress.
+	/// Locking the translations of the pose will remove the visual stretch from the ragdoll but will introduce a difference between the
+	/// physical simulation and the visual representation.
+	/// @param inSkeleton2 Target skeleton to map to.
+	/// @param inLockedTranslations An array of bools the size of inSkeleton2->GetJointCount(), for each joint indicating if the joint is locked.
+	/// @param inNeutralPose2 Neutral pose to take reference translations from
+	void					LockTranslations(const Skeleton *inSkeleton2, const bool *inLockedTranslations, const Mat44 *inNeutralPose2);
+
+	/// After Initialize(), this can be called to lock the translation of all joints in skeleton 2 below the first mapped joint to those of the neutral pose.
+	/// Because constraints are never 100% rigid, there's always a little bit of stretch in the ragdoll when the ragdoll is under stress.
+	/// Locking the translations of the pose will remove the visual stretch from the ragdoll but will introduce a difference between the
+	/// physical simulation and the visual representation.
+	/// @param inSkeleton2 Target skeleton to map to.
+	/// @param inNeutralPose2 Neutral pose to take reference translations from
+	void					LockAllTranslations(const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2);
+
 	/// Map a pose. Joints that were directly mappable will be copied in model space from pose 1 to pose 2. Any joints that are only present in skeleton 2
 	/// will get their model space transform calculated through the local space transforms of pose 2. Joints that are part of a joint chain between two
 	/// mapped joints will be reoriented towards the next joint in skeleton 1. This means that it is possible for unmapped joints to have some animation,
@@ -83,6 +109,7 @@ public:
 	using MappingVector = Array<Mapping>;
 	using ChainVector = Array<Chain>;
 	using UnmappedVector = Array<Unmapped>;
+	using LockedVector = Array<Locked>;
 
 	///@name Access to the mapped joints
 	///@{
@@ -92,6 +119,8 @@ public:
 	ChainVector &			GetChains()																	{ return mChains; }
 	const UnmappedVector &	GetUnmapped() const															{ return mUnmapped; }
 	UnmappedVector &		GetUnmapped()																{ return mUnmapped; }
+	const LockedVector &	GetLockedTranslations() const												{ return mLockedTranslations; }
+	LockedVector &			GetLockedTranslations()														{ return mLockedTranslations; }
 	///@}
 
 private:
@@ -99,6 +128,7 @@ private:
 	MappingVector			mMappings;
 	ChainVector				mChains;
 	UnmappedVector			mUnmapped;																	///< Joint indices that could not be mapped from 1 to 2 (these are indices in 2)
+	LockedVector			mLockedTranslations;
 };
 
 JPH_NAMESPACE_END

Samples/Tests/Rig/SkeletonMapperTest.cpp

@@ -9,6 +9,7 @@
 #include <Renderer/DebugRendererImp.h>
 #include <Layers.h>
 #include <Utils/Log.h>
+#include <Application/DebugUI.h>
 
 JPH_IMPLEMENT_RTTI_VIRTUAL(SkeletonMapperTest) 
 { 
@@ -63,6 +64,11 @@ void SkeletonMapperTest::Initialize()
 	mAnimatedPose.CalculateJointMatrices();
 	mRagdollToAnimated.Initialize(mRagdollPose.GetSkeleton(), mRagdollPose.GetJointMatrices().data(), mAnimatedPose.GetSkeleton(), mAnimatedPose.GetJointMatrices().data());
 
+	// Optionally lock translations (this can be used to prevent ragdolls from stretching)
+	// Try wildly dragging the ragdoll by the head (using spacebar) to see how the ragdoll stretches under stress
+	if (sLockTranslations)
+		mRagdollToAnimated.LockAllTranslations(mAnimatedPose.GetSkeleton(), mAnimatedPose.GetJointMatrices().data());
+
 	// Calculate initial pose and set it
 	CalculateRagdollPose();
 	mRagdoll->SetPose(mRagdollPose);
@@ -119,6 +125,11 @@ void SkeletonMapperTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
 #endif // JPH_DEBUG_RENDERER
 }
 
+void SkeletonMapperTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu)
+{
+	inUI->CreateCheckBox(inSubMenu, "Lock Translations", sLockTranslations, [this](UICheckBox::EState inState) { sLockTranslations = inState == UICheckBox::STATE_CHECKED; RestartTest(); });
+}
+
 void SkeletonMapperTest::SaveState(StateRecorder &inStream) const
 {
 	inStream.Write(mTime);

Samples/Tests/Rig/SkeletonMapperTest.h

@@ -26,11 +26,17 @@ public:
 	virtual void			Initialize() override;
 	virtual void			PrePhysicsUpdate(const PreUpdateParams &inParams) override;
 
+	// Optional settings menu
+	virtual bool			HasSettingsMenu() const override							{ return true; }
+	virtual void			CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu) override;
+
 	// Saving / restoring state for replay
 	virtual void			SaveState(StateRecorder &inStream) const override;
 	virtual void			RestoreState(StateRecorder &inStream) override;
 
 private:
+	inline static bool		sLockTranslations = false;
+	
 	void					CalculateRagdollPose();
 
 	float					mTime = 0.0f;