Merge pull request #89103 from Malcolmnixon/xr-body-hips-height

Fix XR Body crouching and climbing

doc/classes/XRBodyModifier3D.xml

@@ -1,10 +1,12 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<class name="XRBodyModifier3D" inherits="Node3D" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../class.xsd">
+<class name="XRBodyModifier3D" inherits="Node3D" experimental="" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../class.xsd">
 	<brief_description>
 		A node for driving body meshes from [XRBodyTracker] data.
 	</brief_description>
 	<description>
 		This node uses body tracking data from a [XRBodyTracker] to animate the skeleton of a body mesh.
+		This node positions itself at the [constant XRBodyTracker.JOINT_ROOT] position and scales itself to [member XRServer.world_scale]. Adding the body model as a child of this node will result in the model being positioned and scaled correctly for XR experiences.
+		The body tracking position-data is scaled by [member Skeleton3D.motion_scale] when applied to the skeleton, which can be used to adjust the tracked body to match the scale of the body model.
 	</description>
 	<tutorials>
 		<link title="XR documentation index">$DOCS_URL/tutorials/xr/index.html</link>

doc/classes/XRBodyTracker.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<class name="XRBodyTracker" inherits="RefCounted" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../class.xsd">
+<class name="XRBodyTracker" inherits="RefCounted" experimental="" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../class.xsd">
 	<brief_description>
 		A tracked body in XR.
 	</brief_description>

doc/classes/XRFaceModifier3D.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<class name="XRFaceModifier3D" inherits="Node3D" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../class.xsd">
+<class name="XRFaceModifier3D" inherits="Node3D" experimental="" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../class.xsd">
 	<brief_description>
 		A node for driving standard face meshes from [XRFaceTracker] weights.
 	</brief_description>

doc/classes/XRFaceTracker.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<class name="XRFaceTracker" inherits="RefCounted" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../class.xsd">
+<class name="XRFaceTracker" inherits="RefCounted" experimental="" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../class.xsd">
 	<brief_description>
 		A tracked face.
 	</brief_description>

scene/3d/xr_body_modifier_3d.cpp

@@ -311,18 +311,22 @@ void XRBodyModifier3D::_update_skeleton() {
 		return;
 	}
 
-	// Read the relevant tracking data.
+	// Get the world and skeleton scale.
+	const float ws = xr_server->get_world_scale();
+	const float ss = skeleton->get_motion_scale();
+
+	// Read the relevant tracking data. This applies the skeleton motion scale to
+	// the joint transforms, allowing the tracking data to be scaled to the skeleton.
 	bool has_valid_data[XRBodyTracker::JOINT_MAX];
 	Transform3D transforms[XRBodyTracker::JOINT_MAX];
 	Transform3D inv_transforms[XRBodyTracker::JOINT_MAX];
-	const float ws = xr_server->get_world_scale();
 	for (int joint = 0; joint < XRBodyTracker::JOINT_MAX; joint++) {
 		BitField<XRBodyTracker::JointFlags> flags = tracker->get_joint_flags(static_cast<XRBodyTracker::Joint>(joint));
 		has_valid_data[joint] = flags.has_flag(XRBodyTracker::JOINT_FLAG_ORIENTATION_VALID) && flags.has_flag(XRBodyTracker::JOINT_FLAG_POSITION_VALID);
 
 		if (has_valid_data[joint]) {
 			transforms[joint] = tracker->get_joint_transform(static_cast<XRBodyTracker::Joint>(joint));
-			transforms[joint].origin *= ws;
+			transforms[joint].origin *= ss;
 			inv_transforms[joint] = transforms[joint].inverse();
 		}
 	}
@@ -353,17 +357,20 @@ void XRBodyModifier3D::_update_skeleton() {
 		const int parent_joint = joints[joint].parent_joint;
 		const Transform3D relative_transform = inv_transforms[parent_joint] * transforms[joint];
 
-		// Update the bone position if enabled by update mode.
-		if (bone_update == BONE_UPDATE_FULL) {
+		// Update the bone position if enabled by update mode, or if the joint is the hips, to allow
+		// for climbing or crouching.
+		if (bone_update == BONE_UPDATE_FULL || joint == XRBodyTracker::JOINT_HIPS) {
 			skeleton->set_bone_pose_position(joints[joint].bone, relative_transform.origin);
 		}
 
 		// Always update the bone rotation.
 		skeleton->set_bone_pose_rotation(joints[joint].bone, Quaternion(relative_transform.basis));
 	}
 
-	// Transform to the skeleton pose.
-	set_transform(transforms[XRBodyTracker::JOINT_ROOT]);
+	// Transform to the tracking data root pose. This also applies the XR world-scale to allow
+	// scaling the avatars mesh and skeleton appropriately (if they are child nodes).
+	set_transform(
+			transforms[XRBodyTracker::JOINT_ROOT] * ws);
 
 	// If tracking-state determines visibility then show the node.
 	if (show_when_tracked) {