@babylonjs/core 9.9.1 → 9.9.2

package/XR/features/WebXRBodyTracking.pure.js

@@ -584,6 +584,16 @@ export const MixamoAimChildOverrides = {
     ["left-hand-wrist" /* WebXRBodyJoint.LEFT_HAND_WRIST */]: "left-hand-middle-metacarpal" /* WebXRBodyJoint.LEFT_HAND_MIDDLE_METACARPAL */,
     ["right-hand-wrist" /* WebXRBodyJoint.RIGHT_HAND_WRIST */]: "right-hand-middle-metacarpal" /* WebXRBodyJoint.RIGHT_HAND_MIDDLE_METACARPAL */,
 };
+const HandTwistReferenceJoints = {
+    ["left-hand-wrist" /* WebXRBodyJoint.LEFT_HAND_WRIST */]: {
+        first: "left-hand-index-metacarpal" /* WebXRBodyJoint.LEFT_HAND_INDEX_METACARPAL */,
+        second: "left-hand-little-metacarpal" /* WebXRBodyJoint.LEFT_HAND_LITTLE_METACARPAL */,
+    },
+    ["right-hand-wrist" /* WebXRBodyJoint.RIGHT_HAND_WRIST */]: {
+        first: "right-hand-index-metacarpal" /* WebXRBodyJoint.RIGHT_HAND_INDEX_METACARPAL */,
+        second: "right-hand-little-metacarpal" /* WebXRBodyJoint.RIGHT_HAND_LITTLE_METACARPAL */,
+    },
+};
 /**
  * Resolve the Mixamo rig mapping for a given body mesh, auto-detecting the
  * `mixamorig:` bone-name prefix. Falls back to the unprefixed names.
@@ -784,6 +794,8 @@ export class WebXRTrackedBody {
         this._mappedChildBones = new Map();
         /** Bind-space local child direction for each mapped bone. */
         this._bindLocalAimDirections = new Map();
+        /** Bind-space local hand-plane normal used to correct wrist/hand twist from tracked finger positions. */
+        this._bindLocalTwistNormals = new Map();
         /**
          * XR joint index to aim each mapped bone at. This can be a mapped joint
          * (same as `_boneToJointIdx.get(aimChildBone)`) or an **unmapped** XR
@@ -793,6 +805,8 @@ export class WebXRTrackedBody {
          * "where the hand is pointing".
          */
         this._boneAimTargetJointIdx = new Map();
+        /** Per-bone pair of tracked joints that define the hand plane used for twist correction. */
+        this._boneTwistReferenceJointIdx = new Map();
         /**
          * Per-mapped-bone bind-pose world rotation in mesh-local space
          * (decomposed from `bone.getFinalMatrix()` at bind time). Used by the
@@ -838,6 +852,15 @@ export class WebXRTrackedBody {
         this._tempParentAccumRot = new Quaternion();
         /** Scratch quaternion reused for the parent-world intermediate product. */
         this._tempParentAccumTmp = new Quaternion();
+        /** Scratch vectors reused by hand twist correction. */
+        this._tempTwistFirst = new Vector3();
+        this._tempTwistSecond = new Vector3();
+        this._tempTwistNormal = new Vector3();
+        this._tempCurrentTwistNormal = new Vector3();
+        this._tempProjectedTwistNormal = new Vector3();
+        this._tempProjectedDesiredTwistNormal = new Vector3();
+        this._tempTwistAimAxis = new Vector3();
+        this._tempTwistCross = new Vector3();
         /** The skeleton reference for iterating bones in parent-first order. */
         this._skeleton = null;
         /** Cached inverse of the skeleton mesh's world matrix. */
@@ -940,7 +963,9 @@ export class WebXRTrackedBody {
         this._mappedBoneBindLocals.clear();
         this._mappedChildBones.clear();
         this._bindLocalAimDirections.clear();
+        this._bindLocalTwistNormals.clear();
         this._boneAimTargetJointIdx.clear();
+        this._boneTwistReferenceJointIdx.clear();
         this._bindBoneWorldRotMeshLocal.clear();
         this._computedBoneNewWorldRot.clear();
         this._computedBoneNewWorldRotFrameId.clear();
@@ -1023,6 +1048,34 @@ export class WebXRTrackedBody {
                     this._bindBoneWorldRotMeshLocal.set(bone, this._tempRotQuat.clone());
                 }
             }
+            const findBestUnmappedDescendantForJoint = (bone, targetJointName) => {
+                const tokens = targetJointName
+                    .toLowerCase()
+                    .split(/[_\-\s]+/)
+                    .filter((t) => t.length >= 4 && t !== "left" && t !== "right" && t !== "hand" && t !== "foot" && t !== "joint" && t !== "body");
+                let bestDescendant = null;
+                let bestScore = 0;
+                const walk = (b) => {
+                    for (const child of b.children) {
+                        if (!this._boneToJointIdx.has(child)) {
+                            const lname = child.name.toLowerCase();
+                            let score = 0;
+                            for (const t of tokens) {
+                                if (lname.indexOf(t) !== -1) {
+                                    score++;
+                                }
+                            }
+                            if (score > bestScore) {
+                                bestScore = score;
+                                bestDescendant = child;
+                            }
+                            walk(child);
+                        }
+                    }
+                };
+                walk(bone);
+                return bestScore > 0 ? bestDescendant : null;
+            };
             for (const bone of Array.from(this._boneToJointIdx.keys())) {
                 // Prefer an explicit override (XR-joint → XR-joint) when provided.
                 const selfJointIdx = this._boneToJointIdx.get(bone);
@@ -1074,37 +1127,16 @@ export class WebXRTrackedBody {
                 //   offset).
                 if (overrideTargetJointIdx !== -1 && overrideTargetJointName) {
                     this._boneAimTargetJointIdx.set(bone, overrideTargetJointIdx);
-                    // Extract significant tokens from target joint name.
-                    // "LEFT_HAND_MIDDLE_METACARPAL" → ["middle", "metacarpal"]
-                    const tokens = overrideTargetJointName
-                        .toLowerCase()
-                        .split(/[_\-\s]+/)
-                        .filter((t) => t.length >= 4 && t !== "left" && t !== "right" && t !== "hand" && t !== "foot" && t !== "joint" && t !== "body");
-                    // Find best-matching descendant bone: most tokens matched,
-                    // prefer shorter names (closer to the target).
-                    let bestDescendant = null;
-                    let bestScore = 0;
-                    const walk = (b) => {
-                        for (const child of b.children) {
-                            if (!this._boneToJointIdx.has(child)) {
-                                const lname = child.name.toLowerCase();
-                                let score = 0;
-                                for (const t of tokens) {
-                                    if (lname.indexOf(t) !== -1) {
-                                        score++;
-                                    }
-                                }
-                                if (score > bestScore) {
-                                    bestScore = score;
-                                    bestDescendant = child;
-                                }
-                                walk(child);
-                            }
+                    const twistReferences = HandTwistReferenceJoints[selfJointName];
+                    if (twistReferences) {
+                        const firstIdx = BodyJointNameToIndex.get(twistReferences.first);
+                        const secondIdx = BodyJointNameToIndex.get(twistReferences.second);
+                        if (firstIdx !== undefined && secondIdx !== undefined) {
+                            this._boneTwistReferenceJointIdx.set(bone, { first: firstIdx, second: secondIdx });
                         }
-                    };
-                    walk(bone);
-                    if (bestDescendant && bestScore > 0) {
-                        const descendant = bestDescendant;
+                    }
+                    const descendant = findBestUnmappedDescendantForJoint(bone, overrideTargetJointName);
+                    if (descendant) {
                         const boneBindPos = bindWorldPositions.get(bone);
                         // Walk up: descendant's bind world position isn't in
                         // bindWorldPositions (only mapped bones were added).
@@ -1124,6 +1156,18 @@ export class WebXRTrackedBody {
                                 }
                             }
                         }
+                        if (twistReferences && boneBindPos && descBindFinal && bindWorldRotation) {
+                            const firstDescendant = findBestUnmappedDescendantForJoint(bone, twistReferences.first);
+                            const secondDescendant = findBestUnmappedDescendantForJoint(bone, twistReferences.second);
+                            const firstBindFinal = firstDescendant ? bindPoseFinals.get(firstDescendant) : null;
+                            const secondBindFinal = secondDescendant ? bindPoseFinals.get(secondDescendant) : null;
+                            if (firstBindFinal && secondBindFinal) {
+                                descBindFinal.decompose(undefined, undefined, this._tempPosVec);
+                                firstBindFinal.decompose(undefined, undefined, this._tempTwistFirst);
+                                secondBindFinal.decompose(undefined, undefined, this._tempTwistSecond);
+                                this._storeBindLocalTwistNormalFromPositions(bone, boneBindPos, this._tempPosVec, this._tempTwistFirst, this._tempTwistSecond, bindWorldRotation);
+                            }
+                        }
                     }
                     // If we couldn't find a descendant, bind aim will be
                     // computed from tracked-bind positions later (fallback).
@@ -1432,6 +1476,18 @@ export class WebXRTrackedBody {
                 this._tempJointMatrix.multiplyToRef(this._meshWorldMatrixInverse, this._tempLocalMatrix);
                 this._tempLocalMatrix.decompose(undefined, undefined, this._desiredFinalPositions[targetIdx]);
             });
+            this._boneTwistReferenceJointIdx.forEach((twistReferences) => {
+                if (!this._jointHasBone[twistReferences.first]) {
+                    Matrix.FromArrayToRef(this._jointTransformMatrices, twistReferences.first * 16, this._tempJointMatrix);
+                    this._tempJointMatrix.multiplyToRef(this._meshWorldMatrixInverse, this._tempLocalMatrix);
+                    this._tempLocalMatrix.decompose(undefined, undefined, this._desiredFinalPositions[twistReferences.first]);
+                }
+                if (!this._jointHasBone[twistReferences.second]) {
+                    Matrix.FromArrayToRef(this._jointTransformMatrices, twistReferences.second * 16, this._tempJointMatrix);
+                    this._tempJointMatrix.multiplyToRef(this._meshWorldMatrixInverse, this._tempLocalMatrix);
+                    this._tempLocalMatrix.decompose(undefined, undefined, this._desiredFinalPositions[twistReferences.second]);
+                }
+            });
             // Auto-capture bind on first frame when enabled.
             if (!this._hasTrackedBind && this.autoCaptureBindOnFirstFrame) {
                 this._captureTrackedBindFromDesiredFinals();
@@ -1513,6 +1569,45 @@ export class WebXRTrackedBody {
     clearTrackedBind() {
         this._hasTrackedBind = false;
     }
+    _computeNormalFromJointPositions(origin, aimTarget, first, second, result, aimAxisResult) {
+        aimTarget.subtractToRef(origin, aimAxisResult);
+        if (aimAxisResult.lengthSquared() < 1e-8) {
+            return false;
+        }
+        aimAxisResult.normalize();
+        first.subtractToRef(second, this._tempTwistCross);
+        if (this._tempTwistCross.lengthSquared() < 1e-8) {
+            return false;
+        }
+        this._tempTwistCross.normalize();
+        Vector3.CrossToRef(aimAxisResult, this._tempTwistCross, result);
+        if (result.lengthSquared() < 1e-8) {
+            return false;
+        }
+        result.normalize();
+        return true;
+    }
+    _projectOnPlaneToRef(vector, planeNormal, result) {
+        const dot = Vector3.Dot(vector, planeNormal);
+        result.copyFrom(planeNormal).scaleInPlace(-dot).addInPlace(vector);
+        if (result.lengthSquared() < 1e-8) {
+            return false;
+        }
+        result.normalize();
+        return true;
+    }
+    _storeBindLocalTwistNormalFromPositions(bone, origin, aimTarget, first, second, bindWorldRotation) {
+        if (!this._computeNormalFromJointPositions(origin, aimTarget, first, second, this._tempTwistNormal, this._tempTwistAimAxis)) {
+            return;
+        }
+        Quaternion.InverseToRef(bindWorldRotation, this._tempRotQuat2);
+        this._tempTwistNormal.rotateByQuaternionToRef(this._tempRotQuat2, this._tempLocalDirection);
+        if (this._tempLocalDirection.lengthSquared() < 1e-8) {
+            return;
+        }
+        this._tempLocalDirection.normalize();
+        this._bindLocalTwistNormals.set(bone, this._tempLocalDirection.clone());
+    }
     /** Internal: copy current desiredFinals into the tracked-bind slots. */
     _captureTrackedBindFromDesiredFinals() {
         if (!this._trackedBindDesiredFinalRot) {
@@ -1536,6 +1631,14 @@ export class WebXRTrackedBody {
             }
             this._trackedBindDesiredFinalPos[targetIdx].copyFrom(this._desiredFinalPositions[targetIdx]);
         }
+        for (const twistReferences of Array.from(this._boneTwistReferenceJointIdx.values())) {
+            if (!this._jointHasBone[twistReferences.first]) {
+                this._trackedBindDesiredFinalPos[twistReferences.first].copyFrom(this._desiredFinalPositions[twistReferences.first]);
+            }
+            if (!this._jointHasBone[twistReferences.second]) {
+                this._trackedBindDesiredFinalPos[twistReferences.second].copyFrom(this._desiredFinalPositions[twistReferences.second]);
+            }
+        }
         // For any bone whose aim target is UNMAPPED and therefore wasn't
         // resolved at setBodyMesh time, compute its bind-local aim direction
         // now — using tracked bind positions for both endpoints and the
@@ -1562,6 +1665,18 @@ export class WebXRTrackedBody {
             this._tempLocalDirection.normalize();
             this._bindLocalAimDirections.set(bone, this._tempLocalDirection.clone());
         }
+        for (const [bone, twistReferences] of Array.from(this._boneTwistReferenceJointIdx)) {
+            if (this._bindLocalTwistNormals.has(bone)) {
+                continue;
+            }
+            const selfJointIdx = this._boneToJointIdx.get(bone);
+            const targetIdx = this._boneAimTargetJointIdx.get(bone);
+            const bindWorldRot = this._bindBoneWorldRotMeshLocal.get(bone);
+            if (selfJointIdx === undefined || targetIdx === undefined || !bindWorldRot) {
+                continue;
+            }
+            this._storeBindLocalTwistNormalFromPositions(bone, this._trackedBindDesiredFinalPos[selfJointIdx], this._trackedBindDesiredFinalPos[targetIdx], this._trackedBindDesiredFinalPos[twistReferences.first], this._trackedBindDesiredFinalPos[twistReferences.second], bindWorldRot);
+        }
         this._hasTrackedBind = true;
     }
     /**
@@ -1644,6 +1759,28 @@ export class WebXRTrackedBody {
                         }
                     }
                 }
+                const twistReferences = this._boneTwistReferenceJointIdx.get(bone);
+                const bindLocalTwistNormal = this._bindLocalTwistNormals.get(bone);
+                if (twistReferences && bindLocalTwistNormal && targetIdx !== undefined) {
+                    if (this._computeNormalFromJointPositions(this._desiredFinalPositions[jointIdx], this._desiredFinalPositions[targetIdx], this._desiredFinalPositions[twistReferences.first], this._desiredFinalPositions[twistReferences.second], this._tempTwistNormal, this._tempTwistAimAxis)) {
+                        // Rotate bind-space twist normal by the current world orientation,
+                        // then rotate around the aimed axis to match the tracked hand plane.
+                        bindLocalTwistNormal.rotateByQuaternionToRef(this._tempBoneWorldRot, this._tempCurrentTwistNormal);
+                        if (this._projectOnPlaneToRef(this._tempCurrentTwistNormal, this._tempTwistAimAxis, this._tempProjectedTwistNormal) &&
+                            this._projectOnPlaneToRef(this._tempTwistNormal, this._tempTwistAimAxis, this._tempProjectedDesiredTwistNormal)) {
+                            const dot = Vector3.Dot(this._tempProjectedTwistNormal, this._tempProjectedDesiredTwistNormal);
+                            if (dot < -0.9999) {
+                                Quaternion.RotationAxisToRef(this._tempTwistAimAxis, Math.PI, this._tempRotQuat2);
+                            }
+                            else {
+                                Quaternion.FromUnitVectorsToRef(this._tempProjectedTwistNormal, this._tempProjectedDesiredTwistNormal, this._tempRotQuat2);
+                            }
+                            this._tempRotQuat2.multiplyToRef(this._tempBoneWorldRot, this._tempDeltaQuat);
+                            this._tempBoneWorldRot.copyFrom(this._tempDeltaQuat);
+                            this._tempBoneWorldRot.normalize();
+                        }
+                    }
+                }
             }
             // Store for children's parent lookup (reuse pooled quaternion).
             let pooled = this._computedBoneNewWorldRot.get(bone);