Fix Skeleton::commonAncestor to return kInvalidIndex for invalid inputs (#1045)

Summary:
Pull Request resolved: #1045

Previously, commonAncestor(validJoint, kInvalidIndex) would return validJoint, which is incorrect — there is no common ancestor when one of the joints doesn't exist in the hierarchy. The old implementation exited the while loop immediately when either input was kInvalidIndex (due to the `joint != kInvalidIndex` condition) and returned whichever value was in joint1. This was effectively a bug, though no existing callers triggered it since they always passed valid joint indices. The fix adds an early return for kInvalidIndex inputs and simplifies the loop condition.

Reviewed By: cstollmeta

Differential Revision: D93159998

fbshipit-source-id: 32b70761f9250c88192cc4d2e2edde37fa3b1548

Author: cdtwigg

momentum/character/skeleton.cpp

@@ -86,8 +86,11 @@ size_t SkeletonT<T>::commonAncestor(size_t joint1, size_t joint2) const {
   MT_CHECK(joint1 == kInvalidIndex || joint1 < joints.size());
   MT_CHECK(joint2 == kInvalidIndex || joint2 < joints.size());
 
-  while (joint1 != kInvalidIndex && joint2 != kInvalidIndex && joint1 != joint2) {
-    if (joint1 < joint2) {
+  while (joint1 != joint2) {
+    // If either joint is invalid, there is no common ancestor.
+    if (joint1 == kInvalidIndex || joint2 == kInvalidIndex) {
+      return kInvalidIndex;
+    } else if (joint1 < joint2) {
       joint2 = joints[joint2].parent;
     } else {
       joint1 = joints[joint1].parent;

momentum/character/skeleton.h

@@ -64,7 +64,8 @@ struct SkeletonT {
   /// Finds the closest common ancestor of two joints in the hierarchy.
   ///
   /// Returns the index of the joint that is the lowest common ancestor
-  /// in the hierarchy for the two specified joints.
+  /// in the hierarchy for the two specified joints. Returns kInvalidIndex
+  /// if either joint is kInvalidIndex.
   [[nodiscard]] size_t commonAncestor(size_t joint1, size_t joint2) const;
 
   /// Converts the skeleton to use a different scalar type.

momentum/test/character/skeleton_test.cpp

@@ -269,6 +269,54 @@ TYPED_TEST(SkeletonTest, CommonAncestor) {
   // Test common ancestor of cousins
   EXPECT_EQ(skeleton.commonAncestor(2, 4), 0); // Common ancestor of Joint 2 and Joint 4 is Root
   EXPECT_EQ(skeleton.commonAncestor(4, 2), 0); // Common ancestor of Joint 4 and Joint 2 is Root
+
+  // Test with kInvalidIndex — no common ancestor exists
+  EXPECT_EQ(skeleton.commonAncestor(0, kInvalidIndex), kInvalidIndex);
+  EXPECT_EQ(skeleton.commonAncestor(kInvalidIndex, 0), kInvalidIndex);
+  EXPECT_EQ(skeleton.commonAncestor(kInvalidIndex, kInvalidIndex), kInvalidIndex);
+}
+
+// Test commonAncestor with disjoint trees (joints that don't share a root)
+TYPED_TEST(SkeletonTest, CommonAncestorDisjointTrees) {
+  // Create a skeleton with two disjoint trees:
+  //   Joint 0: Root A
+  //   Joint 1: Child of Root A
+  //   Joint 2: Root B (parent = kInvalidIndex, a separate tree)
+  //   Joint 3: Child of Root B
+  std::vector<Joint> disjointJoints(4);
+
+  disjointJoints[0].name = "rootA";
+  disjointJoints[0].parent = kInvalidIndex;
+  disjointJoints[0].translationOffset = Vector3<float>::Zero();
+  disjointJoints[0].preRotation = Quaternion<float>::Identity();
+
+  disjointJoints[1].name = "childA";
+  disjointJoints[1].parent = 0;
+  disjointJoints[1].translationOffset = Vector3<float>(1, 0, 0);
+  disjointJoints[1].preRotation = Quaternion<float>::Identity();
+
+  disjointJoints[2].name = "rootB";
+  disjointJoints[2].parent = kInvalidIndex;
+  disjointJoints[2].translationOffset = Vector3<float>(5, 0, 0);
+  disjointJoints[2].preRotation = Quaternion<float>::Identity();
+
+  disjointJoints[3].name = "childB";
+  disjointJoints[3].parent = 2;
+  disjointJoints[3].translationOffset = Vector3<float>(1, 0, 0);
+  disjointJoints[3].preRotation = Quaternion<float>::Identity();
+
+  using SkeletonType = typename TestFixture::SkeletonType;
+  SkeletonType disjointSkeleton(disjointJoints);
+
+  // Joints within the same tree should find a common ancestor
+  EXPECT_EQ(disjointSkeleton.commonAncestor(0, 1), 0);
+  EXPECT_EQ(disjointSkeleton.commonAncestor(2, 3), 2);
+
+  // Joints across disjoint trees should return kInvalidIndex
+  EXPECT_EQ(disjointSkeleton.commonAncestor(0, 2), kInvalidIndex);
+  EXPECT_EQ(disjointSkeleton.commonAncestor(1, 3), kInvalidIndex);
+  EXPECT_EQ(disjointSkeleton.commonAncestor(0, 3), kInvalidIndex);
+  EXPECT_EQ(disjointSkeleton.commonAncestor(1, 2), kInvalidIndex);
 }
 
 // Test cast method