1 files changed, 122 insertions, 11 deletions

diff --git a/modules/openxr/extensions/openxr_hand_tracking_extension.cpp b/modules/openxr/extensions/openxr_hand_tracking_extension.cpp
index 0d667b56c6..b3c20ef8b9 100644
--- a/modules/openxr/extensions/openxr_hand_tracking_extension.cpp
+++ b/modules/openxr/extensions/openxr_hand_tracking_extension.cpp
@@ -60,6 +60,7 @@ HashMap<String, bool *> OpenXRHandTrackingExtension::get_requested_extensions()
 
 	request_extensions[XR_EXT_HAND_TRACKING_EXTENSION_NAME] = &hand_tracking_ext;
 	request_extensions[XR_EXT_HAND_JOINTS_MOTION_RANGE_EXTENSION_NAME] = &hand_motion_range_ext;
+	request_extensions[XR_EXT_HAND_TRACKING_DATA_SOURCE_EXTENSION_NAME] = &hand_tracking_source_ext;
 
 	return request_extensions;
 }
@@ -137,20 +138,32 @@ void OpenXRHandTrackingExtension::on_process() {
 
 	for (int i = 0; i < OPENXR_MAX_TRACKED_HANDS; i++) {
 		if (hand_trackers[i].hand_tracker == XR_NULL_HANDLE) {
-			XrHandTrackerCreateInfoEXT createInfo = {
+			void *next_pointer = nullptr;
+
+			// Originally not all XR runtimes supported hand tracking data sourced both from controllers and normal hand tracking.
+			// With this extension we can indicate we accept input from both sources so hand tracking data is consistently provided
+			// on runtimes that support this.
+			XrHandTrackingDataSourceEXT data_sources[2] = { XR_HAND_TRACKING_DATA_SOURCE_UNOBSTRUCTED_EXT, XR_HAND_TRACKING_DATA_SOURCE_CONTROLLER_EXT };
+			XrHandTrackingDataSourceInfoEXT data_source_info = { XR_TYPE_HAND_TRACKING_DATA_SOURCE_INFO_EXT, next_pointer, 2, data_sources };
+			if (hand_tracking_source_ext) {
+				// If supported include this info
+				next_pointer = &data_source_info;
+			}
+
+			XrHandTrackerCreateInfoEXT create_info = {
 				XR_TYPE_HAND_TRACKER_CREATE_INFO_EXT, // type
-				nullptr, // next
+				next_pointer, // next
 				i == 0 ? XR_HAND_LEFT_EXT : XR_HAND_RIGHT_EXT, // hand
 				XR_HAND_JOINT_SET_DEFAULT_EXT, // handJointSet
 			};
 
-			result = xrCreateHandTrackerEXT(OpenXRAPI::get_singleton()->get_session(), &createInfo, &hand_trackers[i].hand_tracker);
+			result = xrCreateHandTrackerEXT(OpenXRAPI::get_singleton()->get_session(), &create_info, &hand_trackers[i].hand_tracker);
 			if (XR_FAILED(result)) {
 				// not successful? then we do nothing.
 				print_line("OpenXR: Failed to obtain hand tracking information [", OpenXRAPI::get_singleton()->get_error_string(result), "]");
 				hand_trackers[i].is_initialized = false;
 			} else {
-				void *next_pointer = nullptr;
+				next_pointer = nullptr;
 
 				hand_trackers[i].velocities.type = XR_TYPE_HAND_JOINT_VELOCITIES_EXT;
 				hand_trackers[i].velocities.next = next_pointer;
@@ -158,27 +171,45 @@ void OpenXRHandTrackingExtension::on_process() {
 				hand_trackers[i].velocities.jointVelocities = hand_trackers[i].joint_velocities;
 				next_pointer = &hand_trackers[i].velocities;
 
+				if (hand_tracking_source_ext) {
+					hand_trackers[i].data_source.type = XR_TYPE_HAND_TRACKING_DATA_SOURCE_STATE_EXT;
+					hand_trackers[i].data_source.next = next_pointer;
+					hand_trackers[i].data_source.isActive = false;
+					hand_trackers[i].data_source.dataSource = XrHandTrackingDataSourceEXT(0);
+					next_pointer = &hand_trackers[i].data_source;
+				}
+
+				// Needed for vendor hand tracking extensions implemented from GDExtension.
+				for (OpenXRExtensionWrapper *wrapper : OpenXRAPI::get_singleton()->get_registered_extension_wrappers()) {
+					void *np = wrapper->set_hand_joint_locations_and_get_next_pointer(i, next_pointer);
+					if (np != nullptr) {
+						next_pointer = np;
+					}
+				}
+
 				hand_trackers[i].locations.type = XR_TYPE_HAND_JOINT_LOCATIONS_EXT;
 				hand_trackers[i].locations.next = next_pointer;
 				hand_trackers[i].locations.isActive = false;
 				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 motionRangeInfo;
-
+			XrHandJointsMotionRangeInfoEXT motion_range_info = { XR_TYPE_HAND_JOINTS_MOTION_RANGE_INFO_EXT, next_pointer, hand_trackers[i].motion_range };
 			if (hand_motion_range_ext) {
-				motionRangeInfo.type = XR_TYPE_HAND_JOINTS_MOTION_RANGE_INFO_EXT;
-				motionRangeInfo.next = next_pointer;
-				motionRangeInfo.handJointsMotionRange = hand_trackers[i].motion_range;
-
-				next_pointer = &motionRangeInfo;
+				next_pointer = &motion_range_info;
 			}
 
 			XrHandJointsLocateInfoEXT locateInfo = {
@@ -192,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;
 			}
 
@@ -201,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);
+			}
 		}
 	}
 }
@@ -220,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");
 		}
 	}
 }
@@ -240,6 +332,25 @@ XrHandJointsMotionRangeEXT OpenXRHandTrackingExtension::get_motion_range(HandTra
 	return hand_trackers[p_hand].motion_range;
 }
 
+OpenXRHandTrackingExtension::HandTrackedSource OpenXRHandTrackingExtension::get_hand_tracking_source(HandTrackedHands p_hand) const {
+	ERR_FAIL_UNSIGNED_INDEX_V(p_hand, OPENXR_MAX_TRACKED_HANDS, OPENXR_SOURCE_UNKNOWN);
+
+	if (hand_tracking_source_ext && hand_trackers[p_hand].data_source.isActive) {
+		switch (hand_trackers[p_hand].data_source.dataSource) {
+			case XR_HAND_TRACKING_DATA_SOURCE_UNOBSTRUCTED_EXT:
+				return OPENXR_SOURCE_UNOBSTRUCTED;
+
+			case XR_HAND_TRACKING_DATA_SOURCE_CONTROLLER_EXT:
+				return OPENXR_SOURCE_CONTROLLER;
+
+			default:
+				return OPENXR_SOURCE_UNKNOWN;
+		}
+	}
+
+	return OPENXR_SOURCE_UNKNOWN;
+}
+
 void OpenXRHandTrackingExtension::set_motion_range(HandTrackedHands p_hand, XrHandJointsMotionRangeEXT p_motion_range) {
 	ERR_FAIL_UNSIGNED_INDEX(p_hand, OPENXR_MAX_TRACKED_HANDS);
 	hand_trackers[p_hand].motion_range = p_motion_range;