Merge pull request #86906 from Malcolmnixon/avatar-capable-openxrhand

Add ability to drive full-body avatars using OpenXRHand

modules/openxr/doc_classes/OpenXRHand.xml

@@ -1,10 +1,12 @@
 <?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">
 	<brief_description>
-		Node supporting finger tracking in OpenXR.
+		Node supporting hand and finger tracking in OpenXR.
 	</brief_description>
 	<description>
-		This node enables OpenXR's hand tracking functionality. The node should be a child node of an [XROrigin3D] node, tracking will update its position to where the player's actual hand is positioned. This node also updates the skeleton of a properly skinned hand model. The hand mesh should be a child node of this node.
+		This node enables OpenXR's hand tracking functionality. The node should be a child node of an [XROrigin3D] node, tracking will update its position to the player's tracked hand Palm joint location (the center of the middle finger's metacarpal bone). This node also updates the skeleton of a properly skinned hand or avatar model.
+		If the skeleton is a hand (one of the hand bones is the root node of the skeleton), then the skeleton will be placed relative to the hand palm location and the hand mesh and skeleton should be children of the OpenXRHand node.
+		If the hand bones are part of a full skeleton, then the root of the hand will keep its location with the assumption that IK is used to position the hand and arm.
 	</description>
 	<tutorials>
 	</tutorials>
@@ -18,6 +20,9 @@
 		<member name="motion_range" type="int" setter="set_motion_range" getter="get_motion_range" enum="OpenXRHand.MotionRange" default="0">
 			Set the motion range (if supported) limiting the hand motion.
 		</member>
+		<member name="skeleton_rig" type="int" setter="set_skeleton_rig" getter="get_skeleton_rig" enum="OpenXRHand.SkeletonRig" default="0">
+			Set the type of skeleton rig the [member hand_skeleton] is compliant with.
+		</member>
 	</members>
 	<constants>
 		<constant name="HAND_LEFT" value="0" enum="Hands">
@@ -38,5 +43,14 @@
 		<constant name="MOTION_RANGE_MAX" value="2" enum="MotionRange">
 			Maximum supported motion ranges.
 		</constant>
+		<constant name="SKELETON_RIG_OPENXR" value="0" enum="SkeletonRig">
+			An OpenXR compliant skeleton.
+		</constant>
+		<constant name="SKELETON_RIG_HUMANOID" value="1" enum="SkeletonRig">
+			A [SkeletonProfileHumanoid] compliant skeleton.
+		</constant>
+		<constant name="SKELETON_RIG_MAX" value="2" enum="SkeletonRig">
+			Maximum supported hands.
+		</constant>
 	</constants>
 </class>

modules/openxr/scene/openxr_hand.cpp

@@ -46,9 +46,13 @@ void OpenXRHand::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_motion_range", "motion_range"), &OpenXRHand::set_motion_range);
 	ClassDB::bind_method(D_METHOD("get_motion_range"), &OpenXRHand::get_motion_range);
 
+	ClassDB::bind_method(D_METHOD("set_skeleton_rig", "skeleton_rig"), &OpenXRHand::set_skeleton_rig);
+	ClassDB::bind_method(D_METHOD("get_skeleton_rig"), &OpenXRHand::get_skeleton_rig);
+
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "hand", PROPERTY_HINT_ENUM, "Left,Right"), "set_hand", "get_hand");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "motion_range", PROPERTY_HINT_ENUM, "Unobstructed,Conform to controller"), "set_motion_range", "get_motion_range");
 	ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "hand_skeleton", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Skeleton3D"), "set_hand_skeleton", "get_hand_skeleton");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "skeleton_rig", PROPERTY_HINT_ENUM, "OpenXR,Humanoid"), "set_skeleton_rig", "get_skeleton_rig");
 
 	BIND_ENUM_CONSTANT(HAND_LEFT);
 	BIND_ENUM_CONSTANT(HAND_RIGHT);
@@ -57,6 +61,10 @@ void OpenXRHand::_bind_methods() {
 	BIND_ENUM_CONSTANT(MOTION_RANGE_UNOBSTRUCTED);
 	BIND_ENUM_CONSTANT(MOTION_RANGE_CONFORM_TO_CONTROLLER);
 	BIND_ENUM_CONSTANT(MOTION_RANGE_MAX);
+
+	BIND_ENUM_CONSTANT(SKELETON_RIG_OPENXR);
+	BIND_ENUM_CONSTANT(SKELETON_RIG_HUMANOID);
+	BIND_ENUM_CONSTANT(SKELETON_RIG_MAX);
 }
 
 OpenXRHand::OpenXRHand() {
@@ -64,7 +72,7 @@ OpenXRHand::OpenXRHand() {
 	hand_tracking_ext = OpenXRHandTrackingExtension::get_singleton();
 }
 
-void OpenXRHand::set_hand(const Hands p_hand) {
+void OpenXRHand::set_hand(Hands p_hand) {
 	ERR_FAIL_INDEX(p_hand, HAND_MAX);
 
 	hand = p_hand;
@@ -80,7 +88,7 @@ void OpenXRHand::set_hand_skeleton(const NodePath &p_hand_skeleton) {
 	// TODO if inside tree call _get_bones()
 }
 
-void OpenXRHand::set_motion_range(const MotionRange p_motion_range) {
+void OpenXRHand::set_motion_range(MotionRange p_motion_range) {
 	ERR_FAIL_INDEX(p_motion_range, MOTION_RANGE_MAX);
 	motion_range = p_motion_range;
 
@@ -116,6 +124,16 @@ void OpenXRHand::_set_motion_range() {
 	hand_tracking_ext->set_motion_range(OpenXRHandTrackingExtension::HandTrackedHands(hand), xr_motion_range);
 }
 
+void OpenXRHand::set_skeleton_rig(SkeletonRig p_skeleton_rig) {
+	ERR_FAIL_INDEX(p_skeleton_rig, SKELETON_RIG_MAX);
+
+	skeleton_rig = p_skeleton_rig;
+}
+
+OpenXRHand::SkeletonRig OpenXRHand::get_skeleton_rig() const {
+	return skeleton_rig;
+}
+
 Skeleton3D *OpenXRHand::get_skeleton() {
 	if (!has_node(hand_skeleton)) {
 		return nullptr;
@@ -130,39 +148,81 @@ Skeleton3D *OpenXRHand::get_skeleton() {
 	return skeleton;
 }
 
-void OpenXRHand::_get_bones() {
-	const char *bone_names[XR_HAND_JOINT_COUNT_EXT] = {
-		"Palm",
-		"Wrist",
-		"Thumb_Metacarpal",
-		"Thumb_Proximal",
-		"Thumb_Distal",
-		"Thumb_Tip",
-		"Index_Metacarpal",
-		"Index_Proximal",
-		"Index_Intermediate",
-		"Index_Distal",
-		"Index_Tip",
-		"Middle_Metacarpal",
-		"Middle_Proximal",
-		"Middle_Intermediate",
-		"Middle_Distal",
-		"Middle_Tip",
-		"Ring_Metacarpal",
-		"Ring_Proximal",
-		"Ring_Intermediate",
-		"Ring_Distal",
-		"Ring_Tip",
-		"Little_Metacarpal",
-		"Little_Proximal",
-		"Little_Intermediate",
-		"Little_Distal",
-		"Little_Tip",
+void OpenXRHand::_get_joint_data() {
+	// Table of bone names for different rig types.
+	static const String bone_names[SKELETON_RIG_MAX][XR_HAND_JOINT_COUNT_EXT] = {
+		// SKELETON_RIG_OPENXR bone names.
+		{
+				"Palm",
+				"Wrist",
+				"Thumb_Metacarpal",
+				"Thumb_Proximal",
+				"Thumb_Distal",
+				"Thumb_Tip",
+				"Index_Metacarpal",
+				"Index_Proximal",
+				"Index_Intermediate",
+				"Index_Distal",
+				"Index_Tip",
+				"Middle_Metacarpal",
+				"Middle_Proximal",
+				"Middle_Intermediate",
+				"Middle_Distal",
+				"Middle_Tip",
+				"Ring_Metacarpal",
+				"Ring_Proximal",
+				"Ring_Intermediate",
+				"Ring_Distal",
+				"Ring_Tip",
+				"Little_Metacarpal",
+				"Little_Proximal",
+				"Little_Intermediate",
+				"Little_Distal",
+				"Little_Tip" },
+
+		// SKELETON_RIG_HUMANOID bone names.
+		{
+				"Palm",
+				"Hand",
+				"ThumbMetacarpal",
+				"ThumbProximal",
+				"ThumbDistal",
+				"ThumbTip",
+				"IndexMetacarpal",
+				"IndexProximal",
+				"IndexIntermediate",
+				"IndexDistal",
+				"IndexTip",
+				"MiddleMetacarpal",
+				"MiddleProximal",
+				"MiddleIntermediate",
+				"MiddleDistal",
+				"MiddleTip",
+				"RingMetacarpal",
+				"RingProximal",
+				"RingIntermediate",
+				"RingDistal",
+				"RingTip",
+				"LittleMetacarpal",
+				"LittleProximal",
+				"LittleIntermediate",
+				"LittleDistal",
+				"LittleTip" }
+	};
+
+	// Table of bone name formats for different rig types and left/right hands.
+	static const String bone_name_formats[SKELETON_RIG_MAX][2] = {
+		// SKELETON_RIG_OPENXR bone name format.
+		{ "<bone>_L", "<bone>_R" },
+
+		// SKELETON_RIG_HUMANOID bone name format.
+		{ "Left<bone>", "Right<bone>" }
 	};
 
 	// reset JIC
 	for (int i = 0; i < XR_HAND_JOINT_COUNT_EXT; i++) {
-		bones[i] = -1;
+		joints[i].bone = -1;
+		joints[i].parent_joint = -1;
 	}
 
 	Skeleton3D *skeleton = get_skeleton();
@@ -170,20 +230,46 @@ void OpenXRHand::_get_bones() {
 		return;
 	}
 
-	// We cast to spatials which should allow us to use any subclass of that.
+	// Find the skeleton-bones associated with each OpenXR joint.
+	int bones[XR_HAND_JOINT_COUNT_EXT];
 	for (int i = 0; i < XR_HAND_JOINT_COUNT_EXT; i++) {
-		String bone_name = bone_names[i];
-		if (hand == 0) {
-			bone_name += String("_L");
-		} else {
-			bone_name += String("_R");
-		}
+		// Construct the expected bone name.
+		String bone_name = bone_name_formats[skeleton_rig][hand].replace("<bone>", bone_names[skeleton_rig][i]);
 
+		// Find the skeleton bone.
 		bones[i] = skeleton->find_bone(bone_name);
 		if (bones[i] == -1) {
 			print_line("Couldn't obtain bone for", bone_name);
 		}
 	}
+
+	// Assemble the OpenXR joint relationship to the available skeleton bones.
+	for (int i = 0; i < XR_HAND_JOINT_COUNT_EXT; i++) {
+		// Get the skeleton bone (skip if not found).
+		const int bone = bones[i];
+		if (bone == -1) {
+			continue;
+		}
+
+		// Find the parent skeleton-bone.
+		const int parent_bone = skeleton->get_bone_parent(bone);
+		if (parent_bone == -1) {
+			// If no parent skeleton-bone exists then drive this relative to palm joint.
+			joints[i].bone = bone;
+			joints[i].parent_joint = XR_HAND_JOINT_PALM_EXT;
+			continue;
+		}
+
+		// Find the OpenXR joint associated with the parent skeleton-bone.
+		for (int j = 0; j < XR_HAND_JOINT_COUNT_EXT; ++j) {
+			if (bones[j] == parent_bone) {
+				// If a parent joint is found then drive this bone relative to it.
+				joints[i].bone = bone;
+				joints[i].parent_joint = j;
+				break;
+			}
+		}
+	}
 }
 
 void OpenXRHand::_update_skeleton() {
@@ -198,12 +284,25 @@ void OpenXRHand::_update_skeleton() {
 		return;
 	}
 
+	// Table of bone adjustments for different rig types
+	static const Quaternion bone_adjustments[SKELETON_RIG_MAX] = {
+		// SKELETON_RIG_OPENXR bone adjustment. This is an identity quaternion
+		// because the incoming quaternions are already in OpenXR format.
+		Quaternion(),
+
+		// 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)
+		Quaternion(0.0, -Math_SQRT12, Math_SQRT12, 0.0),
+	};
+
 	// we cache our transforms so we can quickly calculate local transforms
 	XRPose::TrackingConfidence confidences[XR_HAND_JOINT_COUNT_EXT];
 	Quaternion quaternions[XR_HAND_JOINT_COUNT_EXT];
 	Quaternion inv_quaternions[XR_HAND_JOINT_COUNT_EXT];
 	Vector3 positions[XR_HAND_JOINT_COUNT_EXT];
 
+	const Quaternion &rig_adjustment = bone_adjustments[skeleton_rig];
 	const OpenXRHandTrackingExtension::HandTracker *hand_tracker = hand_tracking_ext->get_hand_tracker(OpenXRHandTrackingExtension::HandTrackedHands(hand));
 	const float ws = XRServer::get_singleton()->get_world_scale();
 
@@ -218,7 +317,7 @@ void OpenXRHand::_update_skeleton() {
 
 			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) {
-					quaternions[i] = Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
+					quaternions[i] = Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) * rig_adjustment;
 					inv_quaternions[i] = quaternions[i].inverse();
 
 					if (location.locationFlags & XR_SPACE_LOCATION_POSITION_VALID_BIT) {
@@ -234,40 +333,25 @@ void OpenXRHand::_update_skeleton() {
 		}
 
 		if (confidences[XR_HAND_JOINT_PALM_EXT] != XRPose::XR_TRACKING_CONFIDENCE_NONE) {
-			// now update our skeleton
-			for (int i = 0; i < XR_HAND_JOINT_COUNT_EXT; i++) {
-				if (bones[i] != -1) {
-					int bone = bones[i];
-					int parent = skeleton->get_bone_parent(bone);
-
-					// Get our target quaternion
-					Quaternion q = quaternions[i];
+			// Iterate over all the OpenXR joints.
+			for (int joint = 0; joint < XR_HAND_JOINT_COUNT_EXT; joint++) {
+				// Get the skeleton bone (skip if none).
+				const int bone = joints[joint].bone;
+				if (bone == -1) {
+					continue;
+				}
 
-					// Get our target position
-					Vector3 p = positions[i];
+				// Calculate the relative relationship to the parent bone joint.
+				const int parent_joint = joints[joint].parent_joint;
+				const Quaternion q = inv_quaternions[parent_joint] * quaternions[joint];
+				const Vector3 p = inv_quaternions[parent_joint].xform(positions[joint] - positions[parent_joint]);
 
-					// get local translation, parent should already be processed
-					if (parent == -1) {
-						// use our palm location here, that is what we are tracking
-						q = inv_quaternions[XR_HAND_JOINT_PALM_EXT] * q;
-						p = inv_quaternions[XR_HAND_JOINT_PALM_EXT].xform(p - positions[XR_HAND_JOINT_PALM_EXT]);
-					} else {
-						int found = false;
-						for (int b = 0; b < XR_HAND_JOINT_COUNT_EXT && !found; b++) {
-							if (bones[b] == parent) {
-								q = inv_quaternions[b] * q;
-								p = inv_quaternions[b].xform(p - positions[b]);
-								found = true;
-							}
-						}
-					}
-
-					// and set our pose
-					skeleton->set_bone_pose_position(bones[i], p);
-					skeleton->set_bone_pose_rotation(bones[i], q);
-				}
+				// and set our pose
+				skeleton->set_bone_pose_position(joints[joint].bone, p);
+				skeleton->set_bone_pose_rotation(joints[joint].bone, q);
 			}
 
+			// Transform the OpenXRHand to the skeleton pose.
 			Transform3D t;
 			t.basis = Basis(quaternions[XR_HAND_JOINT_PALM_EXT]);
 			t.origin = positions[XR_HAND_JOINT_PALM_EXT];
@@ -288,7 +372,7 @@ void OpenXRHand::_update_skeleton() {
 void OpenXRHand::_notification(int p_what) {
 	switch (p_what) {
 		case NOTIFICATION_ENTER_TREE: {
-			_get_bones();
+			_get_joint_data();
 
 			set_process_internal(true);
 		} break;
@@ -297,7 +381,8 @@ void OpenXRHand::_notification(int p_what) {
 
 			// reset
 			for (int i = 0; i < XR_HAND_JOINT_COUNT_EXT; i++) {
-				bones[i] = -1;
+				joints[i].bone = -1;
+				joints[i].parent_joint = -1;
 			}
 		} break;
 		case NOTIFICATION_INTERNAL_PROCESS: {

modules/openxr/scene/openxr_hand.h

@@ -55,20 +55,32 @@ public:
 		MOTION_RANGE_MAX
 	};
 
+	enum SkeletonRig {
+		SKELETON_RIG_OPENXR,
+		SKELETON_RIG_HUMANOID,
+		SKELETON_RIG_MAX
+	};
+
 private:
+	struct JointData {
+		int bone = -1;
+		int parent_joint = -1;
+	};
+
 	OpenXRAPI *openxr_api = nullptr;
 	OpenXRHandTrackingExtension *hand_tracking_ext = nullptr;
 
 	Hands hand = HAND_LEFT;
 	MotionRange motion_range = MOTION_RANGE_UNOBSTRUCTED;
 	NodePath hand_skeleton;
+	SkeletonRig skeleton_rig = SKELETON_RIG_OPENXR;
 
-	int64_t bones[XR_HAND_JOINT_COUNT_EXT];
+	JointData joints[XR_HAND_JOINT_COUNT_EXT];
 
 	void _set_motion_range();
 
 	Skeleton3D *get_skeleton();
-	void _get_bones();
+	void _get_joint_data();
 	void _update_skeleton();
 
 protected:
@@ -77,19 +89,23 @@ protected:
 public:
 	OpenXRHand();
 
-	void set_hand(const Hands p_hand);
+	void set_hand(Hands p_hand);
 	Hands get_hand() const;
 
-	void set_motion_range(const MotionRange p_motion_range);
+	void set_motion_range(MotionRange p_motion_range);
 	MotionRange get_motion_range() const;
 
 	void set_hand_skeleton(const NodePath &p_hand_skeleton);
 	NodePath get_hand_skeleton() const;
 
+	void set_skeleton_rig(SkeletonRig p_skeleton_rig);
+	SkeletonRig get_skeleton_rig() const;
+
 	void _notification(int p_what);
 };
 
 VARIANT_ENUM_CAST(OpenXRHand::Hands)
 VARIANT_ENUM_CAST(OpenXRHand::MotionRange)
+VARIANT_ENUM_CAST(OpenXRHand::SkeletonRig)
 
 #endif // OPENXR_HAND_H