Merge pull request #88639 from dsnopek/xrserver-hand-tracker

Provide generic interface for XR hand tracking

4 files changed, 78 insertions, 8 deletions

diff --git a/modules/openxr/doc_classes/OpenXRHand.xml b/modules/openxr/doc_classes/OpenXRHand.xml
index 1c4da83138..23d932ddd7 100644
--- a/modules/openxr/doc_classes/OpenXRHand.xml
+++ b/modules/openxr/doc_classes/OpenXRHand.xml
@@ -1,5 +1,5 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<class name="OpenXRHand" inherits="Node3D" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../../../doc/class.xsd">
+<class name="OpenXRHand" inherits="Node3D" deprecated="Use [XRHandModifier3D] instead." xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../../../doc/class.xsd">
 	<brief_description>
 		Node supporting hand and finger tracking in OpenXR.
 	</brief_description>
diff --git a/modules/openxr/doc_classes/OpenXRInterface.xml b/modules/openxr/doc_classes/OpenXRInterface.xml
index caaaeb90ff..5d38788dcd 100644
--- a/modules/openxr/doc_classes/OpenXRInterface.xml
+++ b/modules/openxr/doc_classes/OpenXRInterface.xml
@@ -23,7 +23,7 @@
 				Returns display refresh rates supported by the current HMD. Only returned if this feature is supported by the OpenXR runtime and after the interface has been initialized.
 			</description>
 		</method>
-		<method name="get_hand_joint_angular_velocity" qualifiers="const">
+		<method name="get_hand_joint_angular_velocity" qualifiers="const" deprecated="Use [method XRHandTracker.get_hand_joint_angular_velocity] obtained from [method XRServer.get_hand_tracker] instead.">
 			<return type="Vector3" />
 			<param index="0" name="hand" type="int" enum="OpenXRInterface.Hand" />
 			<param index="1" name="joint" type="int" enum="OpenXRInterface.HandJoints" />
@@ -31,7 +31,7 @@
 				If handtracking is enabled, returns the angular velocity of a joint ([param joint]) of a hand ([param hand]) as provided by OpenXR. This is relative to [XROrigin3D]!
 			</description>
 		</method>
-		<method name="get_hand_joint_flags" qualifiers="const">
+		<method name="get_hand_joint_flags" qualifiers="const" deprecated="Use [method XRHandTracker.get_hand_joint_flags] obtained from [method XRServer.get_hand_tracker] instead.">
 			<return type="int" enum="OpenXRInterface.HandJointFlags" is_bitfield="true" />
 			<param index="0" name="hand" type="int" enum="OpenXRInterface.Hand" />
 			<param index="1" name="joint" type="int" enum="OpenXRInterface.HandJoints" />
@@ -39,7 +39,7 @@
 				If handtracking is enabled, returns flags that inform us of the validity of the tracking data.
 			</description>
 		</method>
-		<method name="get_hand_joint_linear_velocity" qualifiers="const">
+		<method name="get_hand_joint_linear_velocity" qualifiers="const" deprecated="Use [method XRHandTracker.get_hand_joint_linear_velocity] obtained from [method XRServer.get_hand_tracker] instead.">
 			<return type="Vector3" />
 			<param index="0" name="hand" type="int" enum="OpenXRInterface.Hand" />
 			<param index="1" name="joint" type="int" enum="OpenXRInterface.HandJoints" />
@@ -47,7 +47,7 @@
 				If handtracking is enabled, returns the linear velocity of a joint ([param joint]) of a hand ([param hand]) as provided by OpenXR. This is relative to [XROrigin3D] without worldscale applied!
 			</description>
 		</method>
-		<method name="get_hand_joint_position" qualifiers="const">
+		<method name="get_hand_joint_position" qualifiers="const" deprecated="Use [method XRHandTracker.get_hand_joint_transform] obtained from [method XRServer.get_hand_tracker] instead.">
 			<return type="Vector3" />
 			<param index="0" name="hand" type="int" enum="OpenXRInterface.Hand" />
 			<param index="1" name="joint" type="int" enum="OpenXRInterface.HandJoints" />
@@ -55,7 +55,7 @@
 				If handtracking is enabled, returns the position of a joint ([param joint]) of a hand ([param hand]) as provided by OpenXR. This is relative to [XROrigin3D] without worldscale applied!
 			</description>
 		</method>
-		<method name="get_hand_joint_radius" qualifiers="const">
+		<method name="get_hand_joint_radius" qualifiers="const" deprecated="Use [method XRHandTracker.get_hand_joint_radius] obtained from [method XRServer.get_hand_tracker] instead.">
 			<return type="float" />
 			<param index="0" name="hand" type="int" enum="OpenXRInterface.Hand" />
 			<param index="1" name="joint" type="int" enum="OpenXRInterface.HandJoints" />
@@ -63,7 +63,7 @@
 				If handtracking is enabled, returns the radius of a joint ([param joint]) of a hand ([param hand]) as provided by OpenXR. This is without worldscale applied!
 			</description>
 		</method>
-		<method name="get_hand_joint_rotation" qualifiers="const">
+		<method name="get_hand_joint_rotation" qualifiers="const" deprecated="Use [method XRHandTracker.get_hand_joint_transform] obtained from [method XRServer.get_hand_tracker] instead.">
 			<return type="Quaternion" />
 			<param index="0" name="hand" type="int" enum="OpenXRInterface.Hand" />
 			<param index="1" name="joint" type="int" enum="OpenXRInterface.HandJoints" />
@@ -71,7 +71,7 @@
 				If handtracking is enabled, returns the rotation of a joint ([param joint]) of a hand ([param hand]) as provided by OpenXR.
 			</description>
 		</method>
-		<method name="get_hand_tracking_source" qualifiers="const">
+		<method name="get_hand_tracking_source" qualifiers="const" deprecated="Use [member XRHandTracker.hand_tracking_source] obtained from [method XRServer.get_hand_tracker] instead.">
 			<return type="int" enum="OpenXRInterface.HandTrackedSource" />
 			<param index="0" name="hand" type="int" enum="OpenXRInterface.Hand" />
 			<description>
diff --git a/modules/openxr/extensions/openxr_hand_tracking_extension.cpp b/modules/openxr/extensions/openxr_hand_tracking_extension.cpp
index 884fb41a3c..b3c20ef8b9 100644
--- a/modules/openxr/extensions/openxr_hand_tracking_extension.cpp
+++ b/modules/openxr/extensions/openxr_hand_tracking_extension.cpp
@@ -193,11 +193,18 @@ void OpenXRHandTrackingExtension::on_process() {
 				hand_trackers[i].locations.jointCount = XR_HAND_JOINT_COUNT_EXT;
 				hand_trackers[i].locations.jointLocations = hand_trackers[i].joint_locations;
 
+				Ref<XRHandTracker> godot_tracker;
+				godot_tracker.instantiate();
+				godot_tracker->set_hand(i == 0 ? XRHandTracker::HAND_LEFT : XRHandTracker::HAND_RIGHT);
+				XRServer::get_singleton()->add_hand_tracker(i == 0 ? "/user/left" : "/user/right", godot_tracker);
+				hand_trackers[i].godot_tracker = godot_tracker;
+
 				hand_trackers[i].is_initialized = true;
 			}
 		}
 
 		if (hand_trackers[i].is_initialized) {
+			Ref<XRHandTracker> godot_tracker = hand_trackers[i].godot_tracker;
 			void *next_pointer = nullptr;
 
 			XrHandJointsMotionRangeInfoEXT motion_range_info = { XR_TYPE_HAND_JOINTS_MOTION_RANGE_INFO_EXT, next_pointer, hand_trackers[i].motion_range };
@@ -216,6 +223,7 @@ void OpenXRHandTrackingExtension::on_process() {
 			if (XR_FAILED(result)) {
 				// not successful? then we do nothing.
 				print_line("OpenXR: Failed to get tracking for hand", i, "[", OpenXRAPI::get_singleton()->get_error_string(result), "]");
+				godot_tracker->set_has_tracking_data(false);
 				continue;
 			}
 
@@ -225,6 +233,64 @@ void OpenXRHandTrackingExtension::on_process() {
 					!hand_trackers[i].locations.isActive || isnan(palm.position.x) || palm.position.x < -1000000.00 || palm.position.x > 1000000.00) {
 				hand_trackers[i].locations.isActive = false; // workaround, make sure its inactive
 			}
+
+			if (hand_trackers[i].locations.isActive) {
+				godot_tracker->set_has_tracking_data(true);
+
+				// SKELETON_RIG_HUMANOID bone adjustment. This rotation performs:
+				// OpenXR Z+ -> Godot Humanoid Y-  (Back along the bone)
+				// OpenXR Y+ -> Godot Humanoid Z- (Out the back of the hand)
+				const Quaternion bone_adjustment(0.0, -Math_SQRT12, Math_SQRT12, 0.0);
+
+				for (int joint = 0; joint < XR_HAND_JOINT_COUNT_EXT; joint++) {
+					const XrHandJointLocationEXT &location = hand_trackers[i].joint_locations[joint];
+					const XrHandJointVelocityEXT &velocity = hand_trackers[i].joint_velocities[joint];
+					const XrHandTrackingDataSourceStateEXT &data_source = hand_trackers[i].data_source;
+					const XrPosef &pose = location.pose;
+
+					Transform3D transform;
+					BitField<XRHandTracker::HandJointFlags> flags;
+
+					if (location.locationFlags & XR_SPACE_LOCATION_ORIENTATION_VALID_BIT) {
+						if (pose.orientation.x != 0 || pose.orientation.y != 0 || pose.orientation.z != 0 || pose.orientation.w != 0) {
+							flags.set_flag(XRHandTracker::HAND_JOINT_FLAG_ORIENTATION_VALID);
+							transform.basis = Basis(Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) * bone_adjustment);
+						}
+					}
+					if (location.locationFlags & XR_SPACE_LOCATION_POSITION_VALID_BIT) {
+						flags.set_flag(XRHandTracker::HAND_JOINT_FLAG_POSITION_VALID);
+						transform.origin = Vector3(pose.position.x, pose.position.y, pose.position.z);
+					}
+					if (location.locationFlags & XR_SPACE_LOCATION_ORIENTATION_TRACKED_BIT) {
+						flags.set_flag(XRHandTracker::HAND_JOINT_FLAG_ORIENTATION_TRACKED);
+					}
+					if (location.locationFlags & XR_SPACE_LOCATION_POSITION_TRACKED_BIT) {
+						flags.set_flag(XRHandTracker::HAND_JOINT_FLAG_POSITION_TRACKED);
+					}
+					if (location.locationFlags & XR_SPACE_VELOCITY_LINEAR_VALID_BIT) {
+						flags.set_flag(XRHandTracker::HAND_JOINT_FLAG_LINEAR_VELOCITY_VALID);
+						godot_tracker->set_hand_joint_linear_velocity((XRHandTracker::HandJoint)joint, Vector3(velocity.linearVelocity.x, velocity.linearVelocity.y, velocity.linearVelocity.z));
+					}
+					if (location.locationFlags & XR_SPACE_VELOCITY_ANGULAR_VALID_BIT) {
+						flags.set_flag(XRHandTracker::HAND_JOINT_FLAG_ANGULAR_VELOCITY_VALID);
+						godot_tracker->set_hand_joint_angular_velocity((XRHandTracker::HandJoint)joint, Vector3(velocity.angularVelocity.x, velocity.angularVelocity.y, velocity.angularVelocity.z));
+					}
+
+					godot_tracker->set_hand_joint_flags((XRHandTracker::HandJoint)joint, flags);
+					godot_tracker->set_hand_joint_transform((XRHandTracker::HandJoint)joint, transform);
+					godot_tracker->set_hand_joint_radius((XRHandTracker::HandJoint)joint, location.radius);
+
+					XRHandTracker::HandTrackingSource source = XRHandTracker::HAND_TRACKING_SOURCE_UNKNOWN;
+					if (data_source.dataSource == XR_HAND_TRACKING_DATA_SOURCE_UNOBSTRUCTED_EXT) {
+						source = XRHandTracker::HAND_TRACKING_SOURCE_UNOBSTRUCTED;
+					} else if (data_source.dataSource == XR_HAND_TRACKING_DATA_SOURCE_CONTROLLER_EXT) {
+						source = XRHandTracker::HAND_TRACKING_SOURCE_CONTROLLER;
+					}
+					godot_tracker->set_hand_tracking_source(source);
+				}
+			} else {
+				godot_tracker->set_has_tracking_data(false);
+			}
 		}
 	}
 }
@@ -244,6 +310,8 @@ void OpenXRHandTrackingExtension::cleanup_hand_tracking() {
 
 			hand_trackers[i].is_initialized = false;
 			hand_trackers[i].hand_tracker = XR_NULL_HANDLE;
+
+			XRServer::get_singleton()->remove_hand_tracker(i == 0 ? "/user/left" : "/user/right");
 		}
 	}
 }
diff --git a/modules/openxr/extensions/openxr_hand_tracking_extension.h b/modules/openxr/extensions/openxr_hand_tracking_extension.h
index 967538b377..f709bc05c2 100644
--- a/modules/openxr/extensions/openxr_hand_tracking_extension.h
+++ b/modules/openxr/extensions/openxr_hand_tracking_extension.h
@@ -34,6 +34,7 @@
 #include "../util.h"
 #include "core/math/quaternion.h"
 #include "openxr_extension_wrapper.h"
+#include "servers/xr/xr_hand_tracker.h"
 
 class OpenXRHandTrackingExtension : public OpenXRExtensionWrapper {
 public:
@@ -52,6 +53,7 @@ public:
 
 	struct HandTracker {
 		bool is_initialized = false;
+		Ref<XRHandTracker> godot_tracker;
 		XrHandJointsMotionRangeEXT motion_range = XR_HAND_JOINTS_MOTION_RANGE_UNOBSTRUCTED_EXT;
 		HandTrackedSource source = OPENXR_SOURCE_UNKNOWN;