initial commit, 4.5 stable

thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.cpp

@@ -0,0 +1,165 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Skeleton/SkeletalAnimation.h>
+#include <Jolt/Skeleton/SkeletonPose.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation::JointState)
+{
+	JPH_ADD_ATTRIBUTE(JointState, mRotation)
+	JPH_ADD_ATTRIBUTE(JointState, mTranslation)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation::Keyframe)
+{
+	JPH_ADD_BASE_CLASS(Keyframe, JointState)
+
+	JPH_ADD_ATTRIBUTE(Keyframe, mTime)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation::AnimatedJoint)
+{
+	JPH_ADD_ATTRIBUTE(AnimatedJoint, mJointName)
+	JPH_ADD_ATTRIBUTE(AnimatedJoint, mKeyframes)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation)
+{
+	JPH_ADD_ATTRIBUTE(SkeletalAnimation, mAnimatedJoints)
+	JPH_ADD_ATTRIBUTE(SkeletalAnimation, mIsLooping)
+}
+
+
+void SkeletalAnimation::JointState::FromMatrix(Mat44Arg inMatrix)
+{
+	mRotation = inMatrix.GetQuaternion();
+	mTranslation = inMatrix.GetTranslation();
+}
+
+float SkeletalAnimation::GetDuration() const
+{
+	if (!mAnimatedJoints.empty() && !mAnimatedJoints[0].mKeyframes.empty())
+		return mAnimatedJoints[0].mKeyframes.back().mTime;
+	else
+		return 0.0f;
+}
+
+void SkeletalAnimation::ScaleJoints(float inScale)
+{
+	for (SkeletalAnimation::AnimatedJoint &j : mAnimatedJoints)
+		for (SkeletalAnimation::Keyframe &k : j.mKeyframes)
+			k.mTranslation *= inScale;
+}
+
+void SkeletalAnimation::Sample(float inTime, SkeletonPose &ioPose) const
+{
+	// Correct time when animation is looping
+	JPH_ASSERT(inTime >= 0.0f);
+	float duration = GetDuration();
+	float time = duration > 0.0f && mIsLooping? fmod(inTime, duration) : inTime;
+
+	for (const AnimatedJoint &aj : mAnimatedJoints)
+	{
+		// Do binary search for keyframe
+		int high = (int)aj.mKeyframes.size(), low = -1;
+		while (high - low > 1)
+		{
+			int probe = (high + low) / 2;
+			if (aj.mKeyframes[probe].mTime < time)
+				low = probe;
+			else
+				high = probe;
+		}
+
+		JointState &state = ioPose.GetJoint(ioPose.GetSkeleton()->GetJointIndex(aj.mJointName));
+
+		if (low == -1)
+		{
+			// Before first key, return first key
+			state = static_cast<const JointState &>(aj.mKeyframes.front());
+		}
+		else if (high == (int)aj.mKeyframes.size())
+		{
+			// Beyond last key, return last key
+			state = static_cast<const JointState &>(aj.mKeyframes.back());
+		}
+		else
+		{
+			// Interpolate
+			const Keyframe &s1 = aj.mKeyframes[low];
+			const Keyframe &s2 = aj.mKeyframes[low + 1];
+
+			float fraction = (time - s1.mTime) / (s2.mTime - s1.mTime);
+			JPH_ASSERT(fraction >= 0.0f && fraction <= 1.0f);
+
+			state.mTranslation = (1.0f - fraction) * s1.mTranslation + fraction * s2.mTranslation;
+			JPH_ASSERT(s1.mRotation.IsNormalized());
+			JPH_ASSERT(s2.mRotation.IsNormalized());
+			state.mRotation = s1.mRotation.SLERP(s2.mRotation, fraction);
+			JPH_ASSERT(state.mRotation.IsNormalized());
+		}
+	}
+}
+
+void SkeletalAnimation::SaveBinaryState(StreamOut &inStream) const
+{
+	inStream.Write((uint32)mAnimatedJoints.size());
+	for (const AnimatedJoint &j : mAnimatedJoints)
+	{
+		// Write Joint name and number of keyframes
+		inStream.Write(j.mJointName);
+		inStream.Write((uint32)j.mKeyframes.size());
+		for (const Keyframe &k : j.mKeyframes)
+		{
+			inStream.Write(k.mTime);
+			inStream.Write(k.mRotation);
+			inStream.Write(k.mTranslation);
+		}
+	}
+
+	// Save additional parameters
+	inStream.Write(mIsLooping);
+}
+
+SkeletalAnimation::AnimationResult SkeletalAnimation::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	AnimationResult result;
+
+	Ref<SkeletalAnimation> animation = new SkeletalAnimation;
+
+	// Restore animated joints
+	uint32 len = 0;
+	inStream.Read(len);
+	animation->mAnimatedJoints.resize(len);
+	for (AnimatedJoint &j : animation->mAnimatedJoints)
+	{
+		// Read joint name
+		inStream.Read(j.mJointName);
+
+		// Read keyframes
+		len = 0;
+		inStream.Read(len);
+		j.mKeyframes.resize(len);
+		for (Keyframe &k : j.mKeyframes)
+		{
+			inStream.Read(k.mTime);
+			inStream.Read(k.mRotation);
+			inStream.Read(k.mTranslation);
+		}
+	}
+
+	// Read additional parameters
+	inStream.Read(animation->mIsLooping);
+	result.Set(animation);
+	return result;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Skeleton/SkeletalAnimation.h

@@ -0,0 +1,91 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Core/Result.h>
+#include <Jolt/Core/StreamUtils.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class SkeletonPose;
+
+/// Resource for a skinned animation
+class JPH_EXPORT SkeletalAnimation : public RefTarget<SkeletalAnimation>
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SkeletalAnimation)
+
+public:
+	/// Contains the current state of a joint, a local space transformation relative to its parent joint
+	class JointState
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, JointState)
+
+	public:
+		/// Convert from a local space matrix
+		void							FromMatrix(Mat44Arg inMatrix);
+
+		/// Convert to matrix representation
+		inline Mat44					ToMatrix() const									{ return Mat44::sRotationTranslation(mRotation, mTranslation); }
+
+		Quat							mRotation = Quat::sIdentity();						///< Local space rotation of the joint
+		Vec3							mTranslation = Vec3::sZero();						///< Local space translation of the joint
+	};
+
+	/// Contains the state of a single joint at a particular time
+	class Keyframe : public JointState
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Keyframe)
+
+	public:
+		float							mTime = 0.0f;										///< Time of keyframe in seconds
+	};
+
+	using KeyframeVector = Array<Keyframe>;
+
+	/// Contains the animation for a single joint
+	class AnimatedJoint
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, AnimatedJoint)
+
+	public:
+		String							mJointName;											///< Name of the joint
+		KeyframeVector					mKeyframes;											///< List of keyframes over time
+	};
+
+	using AnimatedJointVector = Array<AnimatedJoint>;
+
+	/// Get the length (in seconds) of this animation
+	float								GetDuration() const;
+
+	/// Scale the size of all joints by inScale
+	void								ScaleJoints(float inScale);
+
+	/// If the animation is looping or not. If an animation is looping, the animation will continue playing after completion
+	void 								SetIsLooping(bool inIsLooping)						{ mIsLooping = inIsLooping; }
+	bool								IsLooping() const									{ return mIsLooping; }
+
+	/// Get the (interpolated) joint transforms at time inTime
+	void								Sample(float inTime, SkeletonPose &ioPose) const;
+
+	/// Get joint samples
+	const AnimatedJointVector &			GetAnimatedJoints() const							{ return mAnimatedJoints; }
+	AnimatedJointVector &				GetAnimatedJoints()									{ return mAnimatedJoints; }
+
+	/// Saves the state of this animation in binary form to inStream.
+	void								SaveBinaryState(StreamOut &inStream) const;
+
+	using AnimationResult = Result<Ref<SkeletalAnimation>>;
+
+	/// Restore a saved ragdoll from inStream
+	static AnimationResult				sRestoreFromBinaryState(StreamIn &inStream);
+
+private:
+	AnimatedJointVector					mAnimatedJoints;									///< List of joints and keyframes
+	bool								mIsLooping = true;									///< If this animation loops back to start
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.cpp

@@ -0,0 +1,82 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Skeleton/Skeleton.h>
+#include <Jolt/ObjectStream/TypeDeclarations.h>
+#include <Jolt/Core/StreamIn.h>
+#include <Jolt/Core/StreamOut.h>
+
+JPH_NAMESPACE_BEGIN
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(Skeleton::Joint)
+{
+	JPH_ADD_ATTRIBUTE(Joint, mName)
+	JPH_ADD_ATTRIBUTE(Joint, mParentName)
+}
+
+JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(Skeleton)
+{
+	JPH_ADD_ATTRIBUTE(Skeleton, mJoints)
+}
+
+int Skeleton::GetJointIndex(const string_view &inName) const
+{
+	for (int i = 0; i < (int)mJoints.size(); ++i)
+		if (mJoints[i].mName == inName)
+			return i;
+
+	return -1;
+}
+
+void Skeleton::CalculateParentJointIndices()
+{
+	for (Joint &j : mJoints)
+		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());
+	for (const Joint &j : mJoints)
+	{
+		inStream.Write(j.mName);
+		inStream.Write(j.mParentJointIndex);
+		inStream.Write(j.mParentName);
+	}
+}
+
+Skeleton::SkeletonResult Skeleton::sRestoreFromBinaryState(StreamIn &inStream)
+{
+	Ref<Skeleton> skeleton = new Skeleton;
+
+	uint32 len = 0;
+	inStream.Read(len);
+	skeleton->mJoints.resize(len);
+	for (Joint &j : skeleton->mJoints)
+	{
+		inStream.Read(j.mName);
+		inStream.Read(j.mParentJointIndex);
+		inStream.Read(j.mParentName);
+	}
+
+	SkeletonResult result;
+	if (inStream.IsEOF() || inStream.IsFailed())
+		result.SetError("Failed to read skeleton from stream");
+	else
+		result.Set(skeleton);
+	return result;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Skeleton/Skeleton.h

@@ -0,0 +1,72 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Core/Result.h>
+#include <Jolt/ObjectStream/SerializableObject.h>
+
+JPH_NAMESPACE_BEGIN
+
+class StreamIn;
+class StreamOut;
+
+/// Resource that contains the joint hierarchy for a skeleton
+class JPH_EXPORT Skeleton : public RefTarget<Skeleton>
+{
+	JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Skeleton)
+
+public:
+	using SkeletonResult = Result<Ref<Skeleton>>;
+
+	/// Declare internal structure for a joint
+	class Joint
+	{
+		JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Joint)
+
+	public:
+							Joint() = default;
+							Joint(const string_view &inName, const string_view &inParentName, int inParentJointIndex) : mName(inName), mParentName(inParentName), mParentJointIndex(inParentJointIndex) { }
+
+		String				mName;																		///< Name of the joint
+		String				mParentName;																///< Name of parent joint
+		int					mParentJointIndex = -1;														///< Index of parent joint (in mJoints) or -1 if it has no parent
+	};
+
+	using JointVector = Array<Joint>;
+
+	///@name Access to the joints
+	///@{
+	const JointVector &		GetJoints() const															{ return mJoints; }
+	JointVector &			GetJoints()																	{ return mJoints; }
+	int						GetJointCount() const														{ return (int)mJoints.size(); }
+	const Joint &			GetJoint(int inJoint) const													{ return mJoints[inJoint]; }
+	Joint &					GetJoint(int inJoint)														{ return mJoints[inJoint]; }
+	uint					AddJoint(const string_view &inName, const string_view &inParentName = string_view()) { mJoints.emplace_back(inName, inParentName, -1); return (uint)mJoints.size() - 1; }
+	uint					AddJoint(const string_view &inName, int inParentIndex)						{ mJoints.emplace_back(inName, inParentIndex >= 0? mJoints[inParentIndex].mName : String(), inParentIndex); return (uint)mJoints.size() - 1; }
+	///@}
+
+	/// Find joint by name
+	int						GetJointIndex(const string_view &inName) const;
+
+	/// 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;
+
+	/// Restore the state of this object from inStream.
+	static SkeletonResult	sRestoreFromBinaryState(StreamIn &inStream);
+
+private:
+	/// Joints
+	JointVector				mJoints;
+};
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.cpp

@@ -0,0 +1,237 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Skeleton/SkeletonMapper.h>
+
+JPH_NAMESPACE_BEGIN
+
+void SkeletonMapper::Initialize(const Skeleton *inSkeleton1, const Mat44 *inNeutralPose1, const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2, const CanMapJoint &inCanMapJoint)
+{
+	JPH_ASSERT(mMappings.empty() && mChains.empty() && mUnmapped.empty()); // Should not be initialized yet
+
+	// Count joints
+	int n1 = inSkeleton1->GetJointCount();
+	int n2 = inSkeleton2->GetJointCount();
+	JPH_ASSERT(n1 <= n2, "Skeleton 1 should be the low detail skeleton!");
+
+	// Keep track of mapped joints (initialize to false)
+	Array<bool> mapped1(n1, false);
+	Array<bool> mapped2(n2, false);
+
+	// Find joints that can be mapped directly
+	for (int j1 = 0; j1 < n1; ++j1)
+		for (int j2 = 0; j2 < n2; ++j2)
+			if (inCanMapJoint(inSkeleton1, j1, inSkeleton2, j2))
+			{
+				// Calculate the transform that takes this joint from skeleton 1 to 2
+				Mat44 joint_1_to_2 = inNeutralPose1[j1].Inversed() * inNeutralPose2[j2];
+
+				// Ensure bottom right element is 1 (numerical imprecision in the inverse can make this not so)
+				joint_1_to_2(3, 3) = 1.0f;
+
+				mMappings.emplace_back(j1, j2, joint_1_to_2);
+				mapped1[j1] = true;
+				mapped2[j2] = true;
+				break;
+			}
+
+	Array<int> cur_chain; // Taken out of the loop to minimize amount of allocations
+
+	// Find joint chains
+	for (int m1 = 0; m1 < (int)mMappings.size(); ++m1)
+	{
+		Array<int> chain2;
+		int chain2_m = -1;
+
+		for (int m2 = m1 + 1; m2 < (int)mMappings.size(); ++m2)
+		{
+			// Find the chain from back from m2 to m1
+			int start = mMappings[m1].mJointIdx2;
+			int end = mMappings[m2].mJointIdx2;
+			int cur = end;
+			cur_chain.clear(); // Should preserve memory
+			do
+			{
+				cur_chain.push_back(cur);
+				cur = inSkeleton2->GetJoint(cur).mParentJointIndex;
+			}
+			while (cur >= 0 && cur != start && !mapped2[cur]);
+			cur_chain.push_back(start);
+
+			if (cur == start // This should be the correct chain
+				&& cur_chain.size() > 2 // It should have joints between the mapped joints
+				&& cur_chain.size() > chain2.size()) // And it should be the longest so far
+			{
+				chain2.swap(cur_chain);
+				chain2_m = m2;
+			}
+		}
+
+		if (!chain2.empty())
+		{
+			// Get the chain for 1
+			Array<int> chain1;
+			int start = mMappings[m1].mJointIdx1;
+			int cur = mMappings[chain2_m].mJointIdx1;
+			do
+			{
+				chain1.push_back(cur);
+				cur = inSkeleton1->GetJoint(cur).mParentJointIndex;
+			}
+			while (cur >= 0 && cur != start && !mapped1[cur]);
+			chain1.push_back(start);
+
+			// If the chain exists in 1 too
+			if (cur == start)
+			{
+				// Reverse the chains
+				std::reverse(chain1.begin(), chain1.end());
+				std::reverse(chain2.begin(), chain2.end());
+
+				// Mark elements mapped
+				for (int j1 : chain1)
+					mapped1[j1] = true;
+				for (int j2 : chain2)
+					mapped2[j2] = true;
+
+				// Insert the chain
+				mChains.emplace_back(std::move(chain1), std::move(chain2));
+			}
+		}
+	}
+
+	// Collect unmapped joints from 2
+	for (int j2 = 0; j2 < n2; ++j2)
+		if (!mapped2[j2])
+			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
+	for (const Mapping &m : mMappings)
+		outPose2ModelSpace[m.mJointIdx2] = inPose1ModelSpace[m.mJointIdx1] * m.mJoint1To2;
+
+	// Apply chain mappings
+	for (const Chain &c : mChains)
+	{
+		// Calculate end of chain given local space transforms of the joints of the chain
+		Mat44 &chain_start = outPose2ModelSpace[c.mJointIndices2.front()];
+		Mat44 chain_end = chain_start;
+		for (int j = 1; j < (int)c.mJointIndices2.size(); ++j)
+			chain_end = chain_end * inPose2LocalSpace[c.mJointIndices2[j]];
+
+		// Calculate the direction in world space for skeleton 1 and skeleton 2 and the rotation between them
+		Vec3 actual = chain_end.GetTranslation() - chain_start.GetTranslation();
+		Vec3 desired = inPose1ModelSpace[c.mJointIndices1.back()].GetTranslation() - inPose1ModelSpace[c.mJointIndices1.front()].GetTranslation();
+		Quat rotation = Quat::sFromTo(actual, desired);
+
+		// Rotate the start of the chain
+		chain_start.SetRotation(Mat44::sRotation(rotation) * chain_start.GetRotation());
+
+		// Update all joints but the first and the last joint using their local space transforms
+		for (int j = 1; j < (int)c.mJointIndices2.size() - 1; ++j)
+		{
+			int parent = c.mJointIndices2[j - 1];
+			int child = c.mJointIndices2[j];
+			outPose2ModelSpace[child] = outPose2ModelSpace[parent] * inPose2LocalSpace[child];
+		}
+	}
+
+	// All unmapped joints take the local pose and convert it to model space
+	for (const Unmapped &u : mUnmapped)
+		if (u.mParentJointIdx >= 0)
+		{
+			JPH_ASSERT(u.mParentJointIdx < u.mJointIdx, "Joints must be ordered: parents first");
+			outPose2ModelSpace[u.mJointIdx] = outPose2ModelSpace[u.mParentJointIdx] * inPose2LocalSpace[u.mJointIdx];
+		}
+		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
+{
+	// Normally each joint in skeleton 1 should be present in the mapping, so we only need to apply the direct mappings
+	for (const Mapping &m : mMappings)
+		outPose1ModelSpace[m.mJointIdx1] = inPose2ModelSpace[m.mJointIdx2] * m.mJoint2To1;
+}
+
+int SkeletonMapper::GetMappedJointIdx(int inJoint1Idx) const
+{
+	for (const Mapping &m : mMappings)
+		if (m.mJointIdx1 == inJoint1Idx)
+			return m.mJointIdx2;
+
+	return -1;
+}
+
+bool SkeletonMapper::IsJointTranslationLocked(int inJoint2Idx) const
+{
+	for (const Locked &l : mLockedTranslations)
+		if (l.mJointIdx == inJoint2Idx)
+			return true;
+
+	return false;
+}
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Skeleton/SkeletonMapper.h

@@ -0,0 +1,145 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/Reference.h>
+#include <Jolt/Skeleton/Skeleton.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that is able to map a low detail (ragdoll) skeleton to a high detail (animation) skeleton and vice versa
+class JPH_EXPORT SkeletonMapper : public RefTarget<SkeletonMapper>
+{
+public:
+	/// A joint that maps 1-on-1 to a joint in the other skeleton
+	class Mapping
+	{
+	public:
+							Mapping() = default;
+							Mapping(int inJointIdx1, int inJointIdx2, Mat44Arg inJoint1To2) : mJointIdx1(inJointIdx1), mJointIdx2(inJointIdx2), mJoint1To2(inJoint1To2), mJoint2To1(inJoint1To2.Inversed())
+		{
+			// Ensure bottom right element is 1 (numerical imprecision in the inverse can make this not so)
+			mJoint2To1(3, 3) = 1.0f;
+		}
+
+		int					mJointIdx1;																	///< Index of joint from skeleton 1
+		int					mJointIdx2;																	///< Corresponding index of joint from skeleton 2
+		Mat44				mJoint1To2;																	///< Transforms this joint from skeleton 1 to 2
+		Mat44				mJoint2To1;																	///< Inverse of the transform above
+	};
+
+	/// A joint chain that starts with a 1-on-1 mapped joint and ends with a 1-on-1 mapped joint with intermediate joints that cannot be mapped
+	class Chain
+	{
+	public:
+							Chain() = default;
+							Chain(Array<int> &&inJointIndices1, Array<int> &&inJointIndices2) : mJointIndices1(std::move(inJointIndices1)), mJointIndices2(std::move(inJointIndices2)) { }
+
+		Array<int>			mJointIndices1;																///< Joint chain from skeleton 1
+		Array<int>			mJointIndices2;																///< Corresponding joint chain from skeleton 2
+	};
+
+	/// Joints that could not be mapped from skeleton 1 to 2
+	class Unmapped
+	{
+	public:
+							Unmapped() = default;
+							Unmapped(int inJointIdx, int inParentJointIdx) : mJointIdx(inJointIdx), mParentJointIdx(inParentJointIdx) { }
+
+		int					mJointIdx;																	///< Joint index of unmappable joint
+		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 (in skeleton 2)
+		int					mParentJointIdx;															///< Parent joint index of joint with locked translation (in skeleton 2)
+		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)>;
+
+	/// Default function that checks if the names of the joints are equal
+	static bool				sDefaultCanMapJoint(const Skeleton *inSkeleton1, int inIndex1, const Skeleton *inSkeleton2, int inIndex2)
+	{
+		return inSkeleton1->GetJoint(inIndex1).mName == inSkeleton2->GetJoint(inIndex2).mName;
+	}
+
+	/// Initialize the skeleton mapper. Skeleton 1 should be the (low detail) ragdoll skeleton and skeleton 2 the (high detail) animation skeleton.
+	/// We assume that each joint in skeleton 1 can be mapped to a joint in skeleton 2 (if not mapping from animation skeleton to ragdoll skeleton will be undefined).
+	/// Skeleton 2 should have the same hierarchy as skeleton 1 but can contain extra joints between those in skeleton 1 and it can have extra joints at the root and leaves of the skeleton.
+	/// @param inSkeleton1 Source skeleton to map from.
+	/// @param inNeutralPose1 Neutral pose of the source skeleton (model space)
+	/// @param inSkeleton2 Target skeleton to map to.
+	/// @param inNeutralPose2 Neutral pose of the target skeleton (model space), inNeutralPose1 and inNeutralPose2 must match as closely as possible, preferably the position of the mappable joints should be identical.
+	/// @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,
+	/// but very extreme animation poses will show artifacts.
+	/// @param inPose1ModelSpace Pose on skeleton 1 in model space
+	/// @param inPose2LocalSpace Pose on skeleton 2 in local space (used for the joints that cannot be mapped)
+	/// @param outPose2ModelSpace Model space pose on skeleton 2 (the output of the mapping)
+	void					Map(const Mat44 *inPose1ModelSpace, const Mat44 *inPose2LocalSpace, Mat44 *outPose2ModelSpace) const;
+
+	/// Reverse map a pose, this will only use the mappings and not the chains (it assumes that all joints in skeleton 1 are mapped)
+	/// @param inPose2ModelSpace Model space pose on skeleton 2
+	/// @param outPose1ModelSpace When the function returns this will contain the model space pose for skeleton 1
+	void					MapReverse(const Mat44 *inPose2ModelSpace, Mat44 *outPose1ModelSpace) const;
+
+	/// Search through the directly mapped joints (mMappings) and find inJoint1Idx, returns the corresponding Joint2Idx or -1 if not found.
+	int						GetMappedJointIdx(int inJoint1Idx) const;
+
+	/// Search through the locked translations (mLockedTranslations) and find if joint inJoint2Idx is locked.
+	bool					IsJointTranslationLocked(int inJoint2Idx) const;
+
+	using MappingVector = Array<Mapping>;
+	using ChainVector = Array<Chain>;
+	using UnmappedVector = Array<Unmapped>;
+	using LockedVector = Array<Locked>;
+
+	///@name Access to the mapped joints
+	///@{
+	const MappingVector &	GetMappings() const															{ return mMappings; }
+	MappingVector &			GetMappings()																{ return mMappings; }
+	const ChainVector &		GetChains() const															{ return mChains; }
+	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:
+	/// Joint mappings
+	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

thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.cpp

@@ -0,0 +1,87 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include <Jolt/Jolt.h>
+
+#include <Jolt/Skeleton/SkeletonPose.h>
+#ifdef JPH_DEBUG_RENDERER
+	#include <Jolt/Renderer/DebugRenderer.h>
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_BEGIN
+
+void SkeletonPose::SetSkeleton(const Skeleton *inSkeleton)
+{
+	mSkeleton = inSkeleton;
+
+	mJoints.resize(mSkeleton->GetJointCount());
+	mJointMatrices.resize(mSkeleton->GetJointCount());
+}
+
+void SkeletonPose::CalculateJointMatrices()
+{
+	for (int i = 0; i < (int)mJoints.size(); ++i)
+	{
+		mJointMatrices[i] = mJoints[i].ToMatrix();
+
+		int parent = mSkeleton->GetJoint(i).mParentJointIndex;
+		if (parent >= 0)
+		{
+			JPH_ASSERT(parent < i, "Joints must be ordered: parents first");
+			mJointMatrices[i] = mJointMatrices[parent] * mJointMatrices[i];
+		}
+	}
+}
+
+void SkeletonPose::CalculateJointStates()
+{
+	for (int i = 0; i < (int)mJoints.size(); ++i)
+	{
+		Mat44 local_transform;
+		int parent = mSkeleton->GetJoint(i).mParentJointIndex;
+		if (parent >= 0)
+			local_transform = mJointMatrices[parent].Inversed() * mJointMatrices[i];
+		else
+			local_transform = mJointMatrices[i];
+
+		JointState &joint = mJoints[i];
+		joint.mTranslation = local_transform.GetTranslation();
+		joint.mRotation = local_transform.GetQuaternion();
+	}
+}
+
+void SkeletonPose::CalculateLocalSpaceJointMatrices(Mat44 *outMatrices) const
+{
+	for (int i = 0; i < (int)mJoints.size(); ++i)
+		outMatrices[i] = mJoints[i].ToMatrix();
+}
+
+#ifdef JPH_DEBUG_RENDERER
+void SkeletonPose::Draw(const DrawSettings &inDrawSettings, DebugRenderer *inRenderer, RMat44Arg inOffset) const
+{
+	RMat44 offset = inOffset * RMat44::sTranslation(mRootOffset);
+
+	const Skeleton::JointVector &joints = mSkeleton->GetJoints();
+
+	for (int b = 0; b < mSkeleton->GetJointCount(); ++b)
+	{
+		RMat44 joint_transform = offset * mJointMatrices[b];
+
+		if (inDrawSettings.mDrawJoints)
+		{
+			int parent = joints[b].mParentJointIndex;
+			if (parent >= 0)
+				inRenderer->DrawLine(offset * mJointMatrices[parent].GetTranslation(), joint_transform.GetTranslation(), Color::sGreen);
+		}
+
+		if (inDrawSettings.mDrawJointOrientations)
+			inRenderer->DrawCoordinateSystem(joint_transform, 0.05f);
+
+		if (inDrawSettings.mDrawJointNames)
+			inRenderer->DrawText3D(joint_transform.GetTranslation(), joints[b].mName, Color::sWhite, 0.05f);
+	}
+}
+#endif // JPH_DEBUG_RENDERER
+
+JPH_NAMESPACE_END

thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.h

@@ -0,0 +1,82 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Skeleton/Skeleton.h>
+#include <Jolt/Skeleton/SkeletalAnimation.h>
+
+JPH_NAMESPACE_BEGIN
+
+#ifdef JPH_DEBUG_RENDERER
+class DebugRenderer;
+#endif // JPH_DEBUG_RENDERER
+
+/// Instance of a skeleton, contains the pose the current skeleton is in
+class JPH_EXPORT SkeletonPose
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	using JointState = SkeletalAnimation::JointState;
+	using JointStateVector = Array<JointState>;
+	using Mat44Vector = Array<Mat44>;
+
+	///@name Skeleton
+	///@{
+	void						SetSkeleton(const Skeleton *inSkeleton);
+	const Skeleton *			GetSkeleton() const														{ return mSkeleton; }
+	///@}
+
+	/// Extra offset applied to the root (and therefore also to all of its children)
+	void						SetRootOffset(RVec3Arg inOffset)										{ mRootOffset = inOffset; }
+	RVec3						GetRootOffset() const													{ return mRootOffset; }
+
+	///@name Properties of the joints
+	///@{
+	uint						GetJointCount() const													{ return (uint)mJoints.size(); }
+	const JointStateVector &	GetJoints() const														{ return mJoints; }
+	JointStateVector &			GetJoints()																{ return mJoints; }
+	const JointState &			GetJoint(int inJoint) const												{ return mJoints[inJoint]; }
+	JointState &				GetJoint(int inJoint)													{ return mJoints[inJoint]; }
+	///@}
+
+	///@name Joint matrices
+	///@{
+	const Mat44Vector &			GetJointMatrices() const												{ return mJointMatrices; }
+	Mat44Vector &				GetJointMatrices()														{ return mJointMatrices; }
+	const Mat44 &				GetJointMatrix(int inJoint) const										{ return mJointMatrices[inJoint]; }
+	Mat44 &						GetJointMatrix(int inJoint)												{ return mJointMatrices[inJoint]; }
+	///@}
+
+	/// Convert the joint states to joint matrices
+	void						CalculateJointMatrices();
+
+	/// Convert joint matrices to joint states
+	void						CalculateJointStates();
+
+	/// Outputs the joint matrices in local space (ensure that outMatrices has GetJointCount() elements, assumes that values in GetJoints() is up to date)
+	void						CalculateLocalSpaceJointMatrices(Mat44 *outMatrices) const;
+
+#ifdef JPH_DEBUG_RENDERER
+	/// Draw settings
+	struct DrawSettings
+	{
+		bool					mDrawJoints = true;
+		bool					mDrawJointOrientations = true;
+		bool					mDrawJointNames = false;
+	};
+
+	/// Draw current pose
+	void						Draw(const DrawSettings &inDrawSettings, DebugRenderer *inRenderer, RMat44Arg inOffset = RMat44::sIdentity()) const;
+#endif // JPH_DEBUG_RENDERER
+
+private:
+	RefConst<Skeleton>			mSkeleton;																///< Skeleton definition
+	RVec3						mRootOffset { RVec3::sZero() };											///< Extra offset applied to the root (and therefore also to all of its children)
+	JointStateVector			mJoints;																///< Local joint orientations (local to parent Joint)
+	Mat44Vector					mJointMatrices;															///< Local joint matrices (local to world matrix)
+};
+
+JPH_NAMESPACE_END