1 files changed, 85 insertions, 90 deletions

diff --git a/scene/3d/xr_hand_modifier_3d.cpp b/scene/3d/xr_hand_modifier_3d.cpp
index 1e1449b54b..baaa9eee48 100644
--- a/scene/3d/xr_hand_modifier_3d.cpp
+++ b/scene/3d/xr_hand_modifier_3d.cpp
@@ -30,7 +30,6 @@
 
 #include "xr_hand_modifier_3d.h"
 
-#include "scene/3d/skeleton_3d.h"
 #include "servers/xr/xr_pose.h"
 #include "servers/xr_server.h"
 
@@ -38,14 +37,10 @@ void XRHandModifier3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_hand_tracker", "tracker_name"), &XRHandModifier3D::set_hand_tracker);
 	ClassDB::bind_method(D_METHOD("get_hand_tracker"), &XRHandModifier3D::get_hand_tracker);
 
-	ClassDB::bind_method(D_METHOD("set_target", "target"), &XRHandModifier3D::set_target);
-	ClassDB::bind_method(D_METHOD("get_target"), &XRHandModifier3D::get_target);
-
 	ClassDB::bind_method(D_METHOD("set_bone_update", "bone_update"), &XRHandModifier3D::set_bone_update);
 	ClassDB::bind_method(D_METHOD("get_bone_update"), &XRHandModifier3D::get_bone_update);
 
-	ADD_PROPERTY(PropertyInfo(Variant::STRING, "hand_tracker", PROPERTY_HINT_ENUM_SUGGESTION, "/user/left,/user/right"), "set_hand_tracker", "get_hand_tracker");
-	ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "target", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Skeleton3D"), "set_target", "get_target");
+	ADD_PROPERTY(PropertyInfo(Variant::STRING, "hand_tracker", PROPERTY_HINT_ENUM_SUGGESTION, "/user/hand_tracker/left,/user/hand_tracker/right"), "set_hand_tracker", "get_hand_tracker");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "bone_update", PROPERTY_HINT_ENUM, "Full,Rotation Only"), "set_bone_update", "get_bone_update");
 
 	BIND_ENUM_CONSTANT(BONE_UPDATE_FULL);
@@ -61,18 +56,6 @@ StringName XRHandModifier3D::get_hand_tracker() const {
 	return tracker_name;
 }
 
-void XRHandModifier3D::set_target(const NodePath &p_target) {
-	target = p_target;
-
-	if (is_inside_tree()) {
-		_get_joint_data();
-	}
-}
-
-NodePath XRHandModifier3D::get_target() const {
-	return target;
-}
-
 void XRHandModifier3D::set_bone_update(BoneUpdate p_bone_update) {
 	ERR_FAIL_INDEX(p_bone_update, BONE_UPDATE_MAX);
 	bone_update = p_bone_update;
@@ -82,21 +65,16 @@ XRHandModifier3D::BoneUpdate XRHandModifier3D::get_bone_update() const {
 	return bone_update;
 }
 
-Skeleton3D *XRHandModifier3D::get_skeleton() {
-	if (!has_node(target)) {
-		return nullptr;
+void XRHandModifier3D::_get_joint_data() {
+	if (!is_inside_tree()) {
+		return;
 	}
 
-	Node *node = get_node(target);
-	if (!node) {
-		return nullptr;
+	if (has_stored_previous_transforms) {
+		previous_relative_transforms.clear();
+		has_stored_previous_transforms = false;
 	}
 
-	Skeleton3D *skeleton = Object::cast_to<Skeleton3D>(node);
-	return skeleton;
-}
-
-void XRHandModifier3D::_get_joint_data() {
 	// Table of bone names for different rig types.
 	static const String bone_names[XRHandTracker::HAND_JOINT_MAX] = {
 		"Palm",
@@ -138,22 +116,30 @@ void XRHandModifier3D::_get_joint_data() {
 		joints[i].parent_joint = -1;
 	}
 
-	Skeleton3D *skeleton = get_skeleton();
+	const Skeleton3D *skeleton = get_skeleton();
 	if (!skeleton) {
 		return;
 	}
 
-	XRServer *xr_server = XRServer::get_singleton();
+	const XRServer *xr_server = XRServer::get_singleton();
 	if (!xr_server) {
 		return;
 	}
 
-	Ref<XRHandTracker> tracker = xr_server->get_hand_tracker(tracker_name);
+	const Ref<XRHandTracker> tracker = xr_server->get_tracker(tracker_name);
 	if (tracker.is_null()) {
 		return;
 	}
 
-	XRHandTracker::Hand hand = tracker->get_hand();
+	// Verify we have a left or right hand tracker.
+	const XRPositionalTracker::TrackerHand tracker_hand = tracker->get_tracker_hand();
+	if (tracker_hand != XRPositionalTracker::TRACKER_HAND_LEFT &&
+			tracker_hand != XRPositionalTracker::TRACKER_HAND_RIGHT) {
+		return;
+	}
+
+	// Get the hand index (0 = left, 1 = right).
+	const int hand = tracker_hand == XRPositionalTracker::TRACKER_HAND_LEFT ? 0 : 1;
 
 	// Find the skeleton-bones associated with each joint.
 	int bones[XRHandTracker::HAND_JOINT_MAX];
@@ -197,24 +183,40 @@ void XRHandModifier3D::_get_joint_data() {
 	}
 }
 
-void XRHandModifier3D::_update_skeleton() {
+void XRHandModifier3D::_process_modification() {
 	Skeleton3D *skeleton = get_skeleton();
 	if (!skeleton) {
 		return;
 	}
 
-	XRServer *xr_server = XRServer::get_singleton();
+	const XRServer *xr_server = XRServer::get_singleton();
 	if (!xr_server) {
 		return;
 	}
 
-	Ref<XRHandTracker> tracker = xr_server->get_hand_tracker(tracker_name);
+	const Ref<XRHandTracker> tracker = xr_server->get_tracker(tracker_name);
 	if (tracker.is_null()) {
 		return;
 	}
 
+	// Skip if no tracking data
+	if (!tracker->get_has_tracking_data()) {
+		if (!has_stored_previous_transforms) {
+			return;
+		}
+
+		// Apply previous relative transforms if they are stored.
+		for (int joint = 0; joint < XRHandTracker::HAND_JOINT_MAX; joint++) {
+			if (bone_update == BONE_UPDATE_FULL) {
+				skeleton->set_bone_pose_position(joints[joint].bone, previous_relative_transforms[joint].origin);
+			}
+
+			skeleton->set_bone_pose_rotation(joints[joint].bone, Quaternion(previous_relative_transforms[joint].basis));
+		}
+		return;
+	}
+
 	// Get the world and skeleton scale.
-	const float ws = xr_server->get_world_scale();
 	const float ss = skeleton->get_motion_scale();
 
 	// We cache our transforms so we can quickly calculate local transforms.
@@ -222,92 +224,85 @@ void XRHandModifier3D::_update_skeleton() {
 	Transform3D transforms[XRHandTracker::HAND_JOINT_MAX];
 	Transform3D inv_transforms[XRHandTracker::HAND_JOINT_MAX];
 
-	if (tracker->get_has_tracking_data()) {
-		for (int joint = 0; joint < XRHandTracker::HAND_JOINT_MAX; joint++) {
-			BitField<XRHandTracker::HandJointFlags> flags = tracker->get_hand_joint_flags((XRHandTracker::HandJoint)joint);
-			has_valid_data[joint] = flags.has_flag(XRHandTracker::HAND_JOINT_FLAG_ORIENTATION_VALID);
+	for (int joint = 0; joint < XRHandTracker::HAND_JOINT_MAX; joint++) {
+		BitField<XRHandTracker::HandJointFlags> flags = tracker->get_hand_joint_flags((XRHandTracker::HandJoint)joint);
+		has_valid_data[joint] = flags.has_flag(XRHandTracker::HAND_JOINT_FLAG_ORIENTATION_VALID);
 
-			if (has_valid_data[joint]) {
-				transforms[joint] = tracker->get_hand_joint_transform((XRHandTracker::HandJoint)joint);
-				transforms[joint].origin *= ss;
-				inv_transforms[joint] = transforms[joint].inverse();
-			}
+		if (has_valid_data[joint]) {
+			transforms[joint] = tracker->get_hand_joint_transform((XRHandTracker::HandJoint)joint);
+			transforms[joint].origin *= ss;
+			inv_transforms[joint] = transforms[joint].inverse();
 		}
+	}
 
-		if (has_valid_data[XRHandTracker::HAND_JOINT_PALM]) {
-			for (int joint = 0; joint < XRHandTracker::HAND_JOINT_MAX; joint++) {
-				// Get the skeleton bone (skip if none).
-				const int bone = joints[joint].bone;
-				if (bone == -1) {
-					continue;
-				}
-
-				// Calculate the relative relationship to the parent bone joint.
-				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) {
-					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));
-			}
+	// Skip if palm has no tracking data
+	if (!has_valid_data[XRHandTracker::HAND_JOINT_PALM]) {
+		return;
+	}
+
+	if (!has_stored_previous_transforms) {
+		previous_relative_transforms.resize(XRHandTracker::HAND_JOINT_MAX);
+		has_stored_previous_transforms = true;
+	}
+	Transform3D *previous_relative_transforms_ptr = previous_relative_transforms.ptrw();
+
+	for (int joint = 0; joint < XRHandTracker::HAND_JOINT_MAX; joint++) {
+		// Get the skeleton bone (skip if none).
+		const int bone = joints[joint].bone;
+		if (bone == -1) {
+			continue;
+		}
 
-			// Transform to the skeleton pose. This uses the HAND_JOINT_PALM position without skeleton-scaling, as it
-			// must be positioned to match the physical hand location. It is scaled with the world space to match
-			// the scaling done to the camera and eyes.
-			set_transform(
-					tracker->get_hand_joint_transform(XRHandTracker::HAND_JOINT_PALM) * ws);
+		// Calculate the relative relationship to the parent bone joint.
+		const int parent_joint = joints[joint].parent_joint;
+		const Transform3D relative_transform = inv_transforms[parent_joint] * transforms[joint];
+		previous_relative_transforms_ptr[joint] = relative_transform;
 
-			set_visible(true);
-		} else {
-			set_visible(false);
+		// Update the bone position if enabled by update mode.
+		if (bone_update == BONE_UPDATE_FULL) {
+			skeleton->set_bone_pose_position(joints[joint].bone, relative_transform.origin);
 		}
-	} else {
-		set_visible(false);
+
+		// Always update the bone rotation.
+		skeleton->set_bone_pose_rotation(joints[joint].bone, Quaternion(relative_transform.basis));
 	}
 }
 
-void XRHandModifier3D::_tracker_changed(StringName p_tracker_name, const Ref<XRHandTracker> &p_tracker) {
+void XRHandModifier3D::_tracker_changed(StringName p_tracker_name, XRServer::TrackerType p_tracker_type) {
 	if (tracker_name == p_tracker_name) {
 		_get_joint_data();
 	}
 }
 
+void XRHandModifier3D::_skeleton_changed(Skeleton3D *p_old, Skeleton3D *p_new) {
+	_get_joint_data();
+}
+
 void XRHandModifier3D::_notification(int p_what) {
 	switch (p_what) {
 		case NOTIFICATION_ENTER_TREE: {
 			XRServer *xr_server = XRServer::get_singleton();
 			if (xr_server) {
-				xr_server->connect("hand_tracker_added", callable_mp(this, &XRHandModifier3D::_tracker_changed));
-				xr_server->connect("hand_tracker_updated", callable_mp(this, &XRHandModifier3D::_tracker_changed));
-				xr_server->connect("hand_tracker_removed", callable_mp(this, &XRHandModifier3D::_tracker_changed).bind(Ref<XRHandTracker>()));
+				xr_server->connect("tracker_added", callable_mp(this, &XRHandModifier3D::_tracker_changed));
+				xr_server->connect("tracker_updated", callable_mp(this, &XRHandModifier3D::_tracker_changed));
+				xr_server->connect("tracker_removed", callable_mp(this, &XRHandModifier3D::_tracker_changed));
 			}
 
 			_get_joint_data();
-
-			set_process_internal(true);
 		} break;
 		case NOTIFICATION_EXIT_TREE: {
 			XRServer *xr_server = XRServer::get_singleton();
 			if (xr_server) {
-				xr_server->disconnect("hand_tracker_added", callable_mp(this, &XRHandModifier3D::_tracker_changed));
-				xr_server->disconnect("hand_tracker_updated", callable_mp(this, &XRHandModifier3D::_tracker_changed));
-				xr_server->disconnect("hand_tracker_removed", callable_mp(this, &XRHandModifier3D::_tracker_changed).bind(Ref<XRHandTracker>()));
+				xr_server->disconnect("tracker_added", callable_mp(this, &XRHandModifier3D::_tracker_changed));
+				xr_server->disconnect("tracker_updated", callable_mp(this, &XRHandModifier3D::_tracker_changed));
+				xr_server->disconnect("tracker_removed", callable_mp(this, &XRHandModifier3D::_tracker_changed));
 			}
 
-			set_process_internal(false);
-
 			for (int i = 0; i < XRHandTracker::HAND_JOINT_MAX; i++) {
 				joints[i].bone = -1;
 				joints[i].parent_joint = -1;
 			}
 		} break;
-		case NOTIFICATION_INTERNAL_PROCESS: {
-			_update_skeleton();
-		} break;
 		default: {
 		} break;
 	}