Skeleton mapper improvements (#354)

- Fixed numerical precision issue
- Added a helper function to get a mapped joint index
- Added a helper function to check if a joint is translation locked

Jolt/Skeleton/SkeletonMapper.cpp

@@ -25,7 +25,13 @@ void SkeletonMapper::Initialize(const Skeleton *inSkeleton1, const Mat44 *inNeut
 		for (int j2 = 0; j2 < n2; ++j2)
 			if (inCanMapJoint(inSkeleton1, j1, inSkeleton2, j2))
 			{
-				mMappings.emplace_back(j1, j2, inNeutralPose1[j1].Inversed() * inNeutralPose2[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;
@@ -209,4 +215,22 @@ void SkeletonMapper::MapReverse(const Mat44 *inPose2ModelSpace, Mat44 *outPose1M
 		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

Jolt/Skeleton/SkeletonMapper.h

@@ -51,8 +51,8 @@ public:
 	class Locked
 	{
 	public:
-		int					mJointIdx;																	///< Joint index of joint with locked translation
-		int					mParentJointIdx;															///< Parent joint index of joint with locked translation
+		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
 	};
 
@@ -106,6 +106,12 @@ public:
 	/// @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>;