From 57aa7345627075fa4cf3f1de33bcddf67baafe80 Mon Sep 17 00:00:00 2001 From: Malcolm Nixon Date: Sat, 24 Feb 2024 19:20:11 -0500 Subject: [PATCH] Add body tracking support. --- doc/classes/XRBodyModifier3D.xml | 49 ++++ doc/classes/XRBodyTracker.xml | 307 ++++++++++++++++++++++ doc/classes/XRServer.xml | 48 ++++ scene/3d/xr_body_modifier_3d.cpp | 415 ++++++++++++++++++++++++++++++ scene/3d/xr_body_modifier_3d.h | 102 ++++++++ scene/register_scene_types.cpp | 2 + servers/register_server_types.cpp | 2 + servers/xr/xr_body_tracker.cpp | 171 ++++++++++++ servers/xr/xr_body_tracker.h | 170 ++++++++++++ servers/xr_server.cpp | 48 ++++ servers/xr_server.h | 10 + 11 files changed, 1324 insertions(+) create mode 100644 doc/classes/XRBodyModifier3D.xml create mode 100644 doc/classes/XRBodyTracker.xml create mode 100644 scene/3d/xr_body_modifier_3d.cpp create mode 100644 scene/3d/xr_body_modifier_3d.h create mode 100644 servers/xr/xr_body_tracker.cpp create mode 100644 servers/xr/xr_body_tracker.h diff --git a/doc/classes/XRBodyModifier3D.xml b/doc/classes/XRBodyModifier3D.xml new file mode 100644 index 00000000000..4227bc57df2 --- /dev/null +++ b/doc/classes/XRBodyModifier3D.xml @@ -0,0 +1,49 @@ + + + + A node for driving body meshes from [XRBodyTracker] data. + + + This node uses body tracking data from a [XRBodyTracker] to animate the skeleton of a body mesh. + + + $DOCS_URL/tutorials/xr/index.html + + + + The name of the [XRBodyTracker] registered with [XRServer] to obtain the body tracking data from. + + + Specifies the body parts to update. + + + Specifies the type of updates to perform on the bones. + + + If true then the nodes visibility is determined by whether tracking data is available. + + + A [NodePath] to a [Skeleton3D] to animate. + + + + + The skeleton's upper body joints are updated. + + + The skeleton's lower body joints are updated. + + + The skeleton's hand joints are updated. + + + The skeleton's bones are fully updated (both position and rotation) to match the tracked bones. + + + The skeleton's bones are only rotated to align with the tracked bones, preserving bone length. + + + Represents the size of the [enum BoneUpdate] enum. + + + diff --git a/doc/classes/XRBodyTracker.xml b/doc/classes/XRBodyTracker.xml new file mode 100644 index 00000000000..5d45ad11bd1 --- /dev/null +++ b/doc/classes/XRBodyTracker.xml @@ -0,0 +1,307 @@ + + + + A tracked body in XR. + + + A body tracking system will create an instance of this object and add it to the [XRServer]. This tracking system will then obtain skeleton data, convert it to the Godot Humanoid skeleton and store this data on the [XRBodyTracker] object. + Use [XRBodyModifier3D] to animate a body mesh using body tracking data. + + + $DOCS_URL/tutorials/xr/index.html + + + + + + + Returns flags about the validity of the tracking data for the given body joint (see [enum XRBodyTracker.JointFlags]). + + + + + + + Returns the transform for the given body joint. + + + + + + + + Sets flags about the validity of the tracking data for the given body joint. + + + + + + + + Sets the transform for the given body joint. + + + + + + The type of body tracking data captured. + + + If [code]true[/code], the body tracking data is valid. + + + + + Upper body tracking supported. + + + Lower body tracking supported. + + + Hand tracking supported. + + + Root joint. + + + Hips joint. + + + Spine joint. + + + Chest joint. + + + Upper chest joint. + + + Neck joint. + + + Head joint. + + + Head tip joint. + + + Left shoulder joint. + + + Left upper arm joint. + + + Left lower arm joint. + + + Right shoulder joint. + + + Right upper arm joint. + + + Right lower arm joint. + + + Left upper leg joint. + + + Left lower leg joint. + + + Left foot joint. + + + Left toes joint. + + + Right upper leg joint. + + + Right lower leg joint. + + + Right foot joint. + + + Right toes joint. + + + Left hand joint. + + + Left palm joint. + + + Left wrist joint. + + + Left thumb metacarpal joint. + + + Left thumb phalanx proximal joint. + + + Left thumb phalanx distal joint. + + + Left thumb tip joint. + + + Left index finger metacarpal joint. + + + Left index finger phalanx proximal joint. + + + Left index finger phalanx intermediate joint. + + + Left index finger phalanx distal joint. + + + Left index finger tip joint. + + + Left middle finger metacarpal joint. + + + Left middle finger phalanx proximal joint. + + + Left middle finger phalanx intermediate joint. + + + Left middle finger phalanx distal joint. + + + Left middle finger tip joint. + + + Left ring finger metacarpal joint. + + + Left ring finger phalanx proximal joint. + + + Left ring finger phalanx intermediate joint. + + + Left ring finger phalanx distal joint. + + + Left ring finger tip joint. + + + Left pinky finger metacarpal joint. + + + Left pinky finger phalanx proximal joint. + + + Left pinky finger phalanx intermediate joint. + + + Left pinky finger phalanx distal joint. + + + Left pinky finger tip joint. + + + Right hand joint. + + + Right palm joint. + + + Right wrist joint. + + + Right thumb metacarpal joint. + + + Right thumb phalanx proximal joint. + + + Right thumb phalanx distal joint. + + + Right thumb tip joint. + + + Right index finger metacarpal joint. + + + Right index finger phalanx proximal joint. + + + Right index finger phalanx intermediate joint. + + + Right index finger phalanx distal joint. + + + Right index finger tip joint. + + + Right middle finger metacarpal joint. + + + Right middle finger phalanx proximal joint. + + + Right middle finger phalanx intermediate joint. + + + Right middle finger phalanx distal joint. + + + Right middle finger tip joint. + + + Right ring finger metacarpal joint. + + + Right ring finger phalanx proximal joint. + + + Right ring finger phalanx intermediate joint. + + + Right ring finger phalanx distal joint. + + + Right ring finger tip joint. + + + Right pinky finger metacarpal joint. + + + Right pinky finger phalanx proximal joint. + + + Right pinky finger phalanx intermediate joint. + + + Right pinky finger phalanx distal joint. + + + Right pinky finger tip joint. + + + Represents the size of the [enum Joint] enum. + + + The joint's orientation data is valid. + + + The joint's orientation is actively tracked. May not be set if tracking has been temporarily lost. + + + The joint's position data is valid. + + + The joint's position is actively tracked. May not be set if tracking has been temporarily lost. + + + diff --git a/doc/classes/XRServer.xml b/doc/classes/XRServer.xml index 09e14f1b210..671cc8f15c9 100644 --- a/doc/classes/XRServer.xml +++ b/doc/classes/XRServer.xml @@ -10,6 +10,14 @@ $DOCS_URL/tutorials/xr/index.html + + + + + + Registers a new [XRBodyTracker] that tracks the joints of a body. + + @@ -66,6 +74,19 @@ Finds an interface by its [param name]. For example, if your project uses capabilities of an AR/VR platform, you can find the interface for that platform by name and initialize it. + + + + + Returns the [XRBodyTracker] with the given tracker name. + + + + + + Returns a dictionary of the registered body trackers. Each element of the dictionary is a tracker name mapping to the [XRBodyTracker] instance. + + @@ -137,6 +158,13 @@ Returns a dictionary of trackers for [param tracker_types]. + + + + + Removes a registered [XRBodyTracker]. + + @@ -179,6 +207,26 @@ + + + + + Emitted when a new body tracker is added. + + + + + + Emitted when a body tracker is removed. + + + + + + + Emitted when an existing body tracker is updated. + + diff --git a/scene/3d/xr_body_modifier_3d.cpp b/scene/3d/xr_body_modifier_3d.cpp new file mode 100644 index 00000000000..3414ce13ae2 --- /dev/null +++ b/scene/3d/xr_body_modifier_3d.cpp @@ -0,0 +1,415 @@ +/**************************************************************************/ +/* xr_body_modifier_3d.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "xr_body_modifier_3d.h" + +#include "scene/3d/skeleton_3d.h" +#include "servers/xr/xr_pose.h" +#include "servers/xr_server.h" + +void XRBodyModifier3D::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_body_tracker", "tracker_name"), &XRBodyModifier3D::set_body_tracker); + ClassDB::bind_method(D_METHOD("get_body_tracker"), &XRBodyModifier3D::get_body_tracker); + + ClassDB::bind_method(D_METHOD("set_target", "target"), &XRBodyModifier3D::set_target); + ClassDB::bind_method(D_METHOD("get_target"), &XRBodyModifier3D::get_target); + + ClassDB::bind_method(D_METHOD("set_body_update", "body_update"), &XRBodyModifier3D::set_body_update); + ClassDB::bind_method(D_METHOD("get_body_update"), &XRBodyModifier3D::get_body_update); + + ClassDB::bind_method(D_METHOD("set_bone_update", "bone_update"), &XRBodyModifier3D::set_bone_update); + ClassDB::bind_method(D_METHOD("get_bone_update"), &XRBodyModifier3D::get_bone_update); + + ClassDB::bind_method(D_METHOD("set_show_when_tracked", "show"), &XRBodyModifier3D::set_show_when_tracked); + ClassDB::bind_method(D_METHOD("get_show_when_tracked"), &XRBodyModifier3D::get_show_when_tracked); + + ADD_PROPERTY(PropertyInfo(Variant::STRING, "body_tracker", PROPERTY_HINT_ENUM_SUGGESTION, "/user/body"), "set_body_tracker", "get_body_tracker"); + ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "target", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Skeleton3D"), "set_target", "get_target"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "body_update", PROPERTY_HINT_FLAGS, "Upper Body,Lower Body,Hands"), "set_body_update", "get_body_update"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "bone_update", PROPERTY_HINT_ENUM, "Full,Rotation Only"), "set_bone_update", "get_bone_update"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "show_when_tracked"), "set_show_when_tracked", "get_show_when_tracked"); + + BIND_BITFIELD_FLAG(BODY_UPDATE_UPPER_BODY); + BIND_BITFIELD_FLAG(BODY_UPDATE_LOWER_BODY); + BIND_BITFIELD_FLAG(BODY_UPDATE_HANDS); + + BIND_ENUM_CONSTANT(BONE_UPDATE_FULL); + BIND_ENUM_CONSTANT(BONE_UPDATE_ROTATION_ONLY); + BIND_ENUM_CONSTANT(BONE_UPDATE_MAX); +} + +void XRBodyModifier3D::set_body_tracker(const StringName &p_tracker_name) { + tracker_name = p_tracker_name; +} + +StringName XRBodyModifier3D::get_body_tracker() const { + return tracker_name; +} + +void XRBodyModifier3D::set_target(const NodePath &p_target) { + target = p_target; + + if (is_inside_tree()) { + _get_joint_data(); + } +} + +NodePath XRBodyModifier3D::get_target() const { + return target; +} + +void XRBodyModifier3D::set_body_update(BitField p_body_update) { + body_update = p_body_update; +} + +BitField XRBodyModifier3D::get_body_update() const { + return body_update; +} + +void XRBodyModifier3D::set_bone_update(BoneUpdate p_bone_update) { + ERR_FAIL_INDEX(p_bone_update, BONE_UPDATE_MAX); + bone_update = p_bone_update; +} + +XRBodyModifier3D::BoneUpdate XRBodyModifier3D::get_bone_update() const { + return bone_update; +} + +void XRBodyModifier3D::set_show_when_tracked(bool p_show_when_tracked) { + show_when_tracked = p_show_when_tracked; +} + +bool XRBodyModifier3D::get_show_when_tracked() const { + return show_when_tracked; +} + +Skeleton3D *XRBodyModifier3D::get_skeleton() { + if (!has_node(target)) { + return nullptr; + } + + Node *node = get_node(target); + if (!node) { + return nullptr; + } + + Skeleton3D *skeleton = Object::cast_to(node); + return skeleton; +} + +void XRBodyModifier3D::_get_joint_data() { + // Table of Godot Humanoid bone names. + static const String bone_names[XRBodyTracker::JOINT_MAX] = { + "Root", // XRBodyTracker::JOINT_ROOT + + // Upper Body Joints. + "Hips", // XRBodyTracker::JOINT_HIPS + "Spine", // XRBodyTracker::JOINT_SPINE + "Chest", // XRBodyTracker::JOINT_CHEST + "UpperChest", // XRBodyTracker::JOINT_UPPER_CHEST + "Neck", // XRBodyTracker::JOINT_NECK" + "Head", // XRBodyTracker::JOINT_HEAD" + "HeadTip", // XRBodyTracker::JOINT_HEAD_TIP" + "LeftShoulder", // XRBodyTracker::JOINT_LEFT_SHOULDER" + "LeftUpperArm", // XRBodyTracker::JOINT_LEFT_UPPER_ARM" + "LeftLowerArm", // XRBodyTracker::JOINT_LEFT_LOWER_ARM" + "RightShoulder", // XRBodyTracker::JOINT_RIGHT_SHOULDER" + "RightUpperArm", // XRBodyTracker::JOINT_RIGHT_UPPER_ARM" + "RightLowerArm", // XRBodyTracker::JOINT_RIGHT_LOWER_ARM" + + // Lower Body Joints. + "LeftUpperLeg", // XRBodyTracker::JOINT_LEFT_UPPER_LEG + "LeftLowerLeg", // XRBodyTracker::JOINT_LEFT_LOWER_LEG + "LeftFoot", // XRBodyTracker::JOINT_LEFT_FOOT + "LeftToes", // XRBodyTracker::JOINT_LEFT_TOES + "RightUpperLeg", // XRBodyTracker::JOINT_RIGHT_UPPER_LEG + "RightLowerLeg", // XRBodyTracker::JOINT_RIGHT_LOWER_LEG + "RightFoot", // XRBodyTracker::JOINT_RIGHT_FOOT + "RightToes", // XRBodyTracker::JOINT_RIGHT_TOES + + // Left Hand Joints. + "LeftHand", // XRBodyTracker::JOINT_LEFT_HAND + "LeftPalm", // XRBodyTracker::JOINT_LEFT_PALM + "LeftWrist", // XRBodyTracker::JOINT_LEFT_WRIST + "LeftThumbMetacarpal", // XRBodyTracker::JOINT_LEFT_THUMB_METACARPAL + "LeftThumbProximal", // XRBodyTracker::JOINT_LEFT_THUMB_PHALANX_PROXIMAL + "LeftThumbDistal", // XRBodyTracker::JOINT_LEFT_THUMB_PHALANX_DISTAL + "LeftThumbTip", // XRBodyTracker::JOINT_LEFT_THUMB_TIP + "LeftIndexMetacarpal", // XRBodyTracker::JOINT_LEFT_INDEX_FINGER_METACARPAL + "LeftIndexProximal", // XRBodyTracker::JOINT_LEFT_INDEX_FINGER_PHALANX_PROXIMAL + "LeftIndexIntermediate", // XRBodyTracker::JOINT_LEFT_INDEX_FINGER_PHALANX_INTERMEDIATE + "LeftIndexDistal", // XRBodyTracker::JOINT_LEFT_INDEX_FINGER_PHALANX_DISTAL + "LeftIndexTip", // XRBodyTracker::JOINT_LEFT_INDEX_FINGER_TIP + "LeftMiddleMetacarpal", // XRBodyTracker::JOINT_LEFT_MIDDLE_FINGER_METACARPAL + "LeftMiddleProximal", // XRBodyTracker::JOINT_LEFT_MIDDLE_FINGER_PHALANX_PROXIMAL + "LeftMiddleIntermediate", // XRBodyTracker::JOINT_LEFT_MIDDLE_FINGER_PHALANX_INTERMEDIATE + "LeftMiddleDistal", // XRBodyTracker::JOINT_LEFT_MIDDLE_FINGER_PHALANX_DISTAL + "LeftMiddleTip", // XRBodyTracker::JOINT_LEFT_MIDDLE_FINGER_TIP + "LeftRingMetacarpal", // XRBodyTracker::JOINT_LEFT_RING_FINGER_METACARPAL + "LeftRingProximal", // XRBodyTracker::JOINT_LEFT_RING_FINGER_PHALANX_PROXIMAL + "LeftRingIntermediate", // XRBodyTracker::JOINT_LEFT_RING_FINGER_PHALANX_INTERMEDIATE + "LeftRingDistal", // XRBodyTracker::JOINT_LEFT_RING_FINGER_PHALANX_DISTAL + "LeftRingTip", // XRBodyTracker::JOINT_LEFT_RING_FINGER_TIP + "LeftLittleMetacarpal", // XRBodyTracker::JOINT_LEFT_PINKY_FINGER_METACARPAL + "LeftLittleProximal", // XRBodyTracker::JOINT_LEFT_PINKY_FINGER_PHALANX_PROXIMAL + "LeftLittleIntermediate", // XRBodyTracker::JOINT_LEFT_PINKY_FINGER_PHALANX_INTERMEDIATE + "LeftLittleDistal", // XRBodyTracker::JOINT_LEFT_PINKY_FINGER_PHALANX_DISTAL + "LeftLittleTip", // XRBodyTracker::JOINT_LEFT_PINKY_FINGER_TIP + + // Right Hand Joints. + "RightHand", // XRBodyTracker::JOINT_RIGHT_HAND + "RightPalm", // XRBodyTracker::JOINT_RIGHT_PALM + "RightWrist", // XRBodyTracker::JOINT_RIGHT_WRIST + "RightThumbMetacarpal", // XRBodyTracker::JOINT_RIGHT_THUMB_METACARPAL + "RightThumbProximal", // XRBodyTracker::JOINT_RIGHT_THUMB_PHALANX_PROXIMAL + "RightThumbDistal", // XRBodyTracker::JOINT_RIGHT_THUMB_PHALANX_DISTAL + "RightThumbTip", // XRBodyTracker::JOINT_RIGHT_THUMB_TIP + "RightIndexMetacarpal", // XRBodyTracker::JOINT_RIGHT_INDEX_FINGER_METACARPAL + "RightIndexProximal", // XRBodyTracker::JOINT_RIGHT_INDEX_FINGER_PHALANX_PROXIMAL + "RightIndexIntermediate", // XRBodyTracker::JOINT_RIGHT_INDEX_FINGER_PHALANX_INTERMEDIATE + "RightIndexDistal", // XRBodyTracker::JOINT_RIGHT_INDEX_FINGER_PHALANX_DISTAL + "RightIndexTip", // XRBodyTracker::JOINT_RIGHT_INDEX_FINGER_TIP + "RightMiddleMetacarpal", // XRBodyTracker::JOINT_RIGHT_MIDDLE_FINGER_METACARPAL + "RightMiddleProximal", // XRBodyTracker::JOINT_RIGHT_MIDDLE_FINGER_PHALANX_PROXIMAL + "RightMiddleIntermediate", // XRBodyTracker::JOINT_RIGHT_MIDDLE_FINGER_PHALANX_INTERMEDIATE + "RightMiddleDistal", // XRBodyTracker::JOINT_RIGHT_MIDDLE_FINGER_PHALANX_DISTAL + "RightMiddleTip", // XRBodyTracker::JOINT_RIGHT_MIDDLE_FINGER_TIP + "RightRingMetacarpal", // XRBodyTracker::JOINT_RIGHT_RING_FINGER_METACARPAL + "RightRingProximal", // XRBodyTracker::JOINT_RIGHT_RING_FINGER_PHALANX_PROXIMAL + "RightRingIntermediate", // XRBodyTracker::JOINT_RIGHT_RING_FINGER_PHALANX_INTERMEDIATE + "RightRingDistal", // XRBodyTracker::JOINT_RIGHT_RING_FINGER_PHALANX_DISTAL + "RightRingTip", // XRBodyTracker::JOINT_RIGHT_RING_FINGER_TIP + "RightLittleMetacarpal", // XRBodyTracker::JOINT_RIGHT_PINKY_FINGER_METACARPAL + "RightLittleProximal", // XRBodyTracker::JOINT_RIGHT_PINKY_FINGER_PHALANX_PROXIMAL + "RightLittleIntermediate", // XRBodyTracker::JOINT_RIGHT_PINKY_FINGER_PHALANX_INTERMEDIATE + "RightLittleDistal", // XRBodyTracker::JOINT_RIGHT_PINKY_FINGER_PHALANX_DISTAL + "RightLittleTip", // XRBodyTracker::JOINT_RIGHT_PINKY_FINGER_TIP + }; + + // reset JIC. + for (int i = 0; i < XRBodyTracker::JOINT_MAX; i++) { + joints[i].bone = -1; + joints[i].parent_joint = -1; + } + + Skeleton3D *skeleton = get_skeleton(); + if (!skeleton) { + return; + } + + // Find the skeleton-bones associated with each joint. + int bones[XRBodyTracker::JOINT_MAX]; + for (int i = 0; i < XRBodyTracker::JOINT_MAX; i++) { + // Skip upper body joints if not enabled. + if (!body_update.has_flag(BODY_UPDATE_UPPER_BODY) && i >= XRBodyTracker::JOINT_HIPS && i <= XRBodyTracker::JOINT_RIGHT_LOWER_ARM) { + bones[i] = -1; + continue; + } + + // Skip lower body joints if not enabled. + if (!body_update.has_flag(BODY_UPDATE_LOWER_BODY) && i >= XRBodyTracker::JOINT_LEFT_UPPER_LEG && i <= XRBodyTracker::JOINT_RIGHT_TOES) { + bones[i] = -1; + continue; + } + + // Skip hand joints if not enabled. + if (!body_update.has_flag(BODY_UPDATE_HANDS) && i >= XRBodyTracker::JOINT_LEFT_HAND && i <= XRBodyTracker::JOINT_RIGHT_PINKY_FINGER_TIP) { + bones[i] = -1; + continue; + } + + // Find the skeleton bone. + bones[i] = skeleton->find_bone(bone_names[i]); + if (bones[i] == -1) { + WARN_PRINT(vformat("Couldn't obtain bone for %s", bone_names[i])); + } + } + + // If the root bone is not found then use the skeleton root bone. + if (bones[XRBodyTracker::JOINT_ROOT] == -1) { + bones[XRBodyTracker::JOINT_ROOT] = 0; + } + + // Assemble the joint relationship to the available skeleton bones. + for (int i = 0; i < XRBodyTracker::JOINT_MAX; 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 the root joint. + joints[i].bone = bone; + joints[i].parent_joint = XRBodyTracker::JOINT_ROOT; + continue; + } + + // Find the joint associated with the parent skeleton-bone. + for (int j = 0; j < XRBodyTracker::JOINT_MAX; ++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 XRBodyModifier3D::_update_skeleton() { + Skeleton3D *skeleton = get_skeleton(); + if (!skeleton) { + return; + } + + XRServer *xr_server = XRServer::get_singleton(); + if (!xr_server) { + return; + } + + Ref tracker = xr_server->get_body_tracker(tracker_name); + if (tracker.is_null()) { + return; + } + + // Handle no tracking data. + if (!tracker->get_has_tracking_data()) { + // If tracking-state determines visibility then hide the node. + if (show_when_tracked) { + set_visible(false); + } + return; + } + + // Read the relevant tracking data. + 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 flags = tracker->get_joint_flags(static_cast(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(joint)); + transforms[joint].origin *= ws; + inv_transforms[joint] = transforms[joint].inverse(); + } + } + + // Handle root joint not tracked. + if (!has_valid_data[XRBodyTracker::JOINT_ROOT]) { + // If tracking-state determines visibility then hide the node. + if (show_when_tracked) { + set_visible(false); + } + return; + } + + // Apply the joint information to the skeleton. + for (int joint = 0; joint < XRBodyTracker::JOINT_MAX; joint++) { + // Skip if no valid joint data + if (!has_valid_data[joint]) { + continue; + } + + // 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)); + } + + // Transform to the skeleton pose. + set_transform(transforms[XRBodyTracker::JOINT_ROOT]); + + // If tracking-state determines visibility then show the node. + if (show_when_tracked) { + set_visible(true); + } +} + +void XRBodyModifier3D::_tracker_changed(const StringName &p_tracker_name, const Ref &p_tracker) { + if (tracker_name == p_tracker_name) { + _get_joint_data(); + } +} + +void XRBodyModifier3D::_notification(int p_what) { + switch (p_what) { + case NOTIFICATION_ENTER_TREE: { + XRServer *xr_server = XRServer::get_singleton(); + if (xr_server) { + xr_server->connect("body_tracker_added", callable_mp(this, &XRBodyModifier3D::_tracker_changed)); + xr_server->connect("body_tracker_updated", callable_mp(this, &XRBodyModifier3D::_tracker_changed)); + xr_server->connect("body_tracker_removed", callable_mp(this, &XRBodyModifier3D::_tracker_changed).bind(Ref())); + } + + _get_joint_data(); + + set_process_internal(true); + } break; + case NOTIFICATION_EXIT_TREE: { + XRServer *xr_server = XRServer::get_singleton(); + if (xr_server) { + xr_server->disconnect("body_tracker_added", callable_mp(this, &XRBodyModifier3D::_tracker_changed)); + xr_server->disconnect("body_tracker_updated", callable_mp(this, &XRBodyModifier3D::_tracker_changed)); + xr_server->disconnect("body_tracker_removed", callable_mp(this, &XRBodyModifier3D::_tracker_changed).bind(Ref())); + } + + set_process_internal(false); + + for (int i = 0; i < XRBodyTracker::JOINT_MAX; i++) { + joints[i].bone = -1; + joints[i].parent_joint = -1; + } + } break; + case NOTIFICATION_INTERNAL_PROCESS: { + _update_skeleton(); + } break; + default: { + } break; + } +} diff --git a/scene/3d/xr_body_modifier_3d.h b/scene/3d/xr_body_modifier_3d.h new file mode 100644 index 00000000000..89ac69b6b0d --- /dev/null +++ b/scene/3d/xr_body_modifier_3d.h @@ -0,0 +1,102 @@ +/**************************************************************************/ +/* xr_body_modifier_3d.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef XR_BODY_MODIFIER_3D_H +#define XR_BODY_MODIFIER_3D_H + +#include "scene/3d/node_3d.h" +#include "servers/xr/xr_body_tracker.h" + +class Skeleton3D; + +/** + The XRBodyModifier3D node drives a body skeleton using body tracking + data from an XRBodyTracker instance. + */ + +class XRBodyModifier3D : public Node3D { + GDCLASS(XRBodyModifier3D, Node3D); + +public: + enum BodyUpdate { + BODY_UPDATE_UPPER_BODY = 1, + BODY_UPDATE_LOWER_BODY = 2, + BODY_UPDATE_HANDS = 4, + }; + + enum BoneUpdate { + BONE_UPDATE_FULL, + BONE_UPDATE_ROTATION_ONLY, + BONE_UPDATE_MAX + }; + + void set_body_tracker(const StringName &p_tracker_name); + StringName get_body_tracker() const; + + void set_target(const NodePath &p_target); + NodePath get_target() const; + + void set_body_update(BitField p_body_update); + BitField get_body_update() const; + + void set_bone_update(BoneUpdate p_bone_update); + BoneUpdate get_bone_update() const; + + void set_show_when_tracked(bool p_show_when_tracked); + bool get_show_when_tracked() const; + + void _notification(int p_what); + +protected: + static void _bind_methods(); + +private: + struct JointData { + int bone = -1; + int parent_joint = -1; + }; + + StringName tracker_name = "/user/body"; + NodePath target; + BitField body_update = BODY_UPDATE_UPPER_BODY | BODY_UPDATE_LOWER_BODY | BODY_UPDATE_HANDS; + BoneUpdate bone_update = BONE_UPDATE_FULL; + bool show_when_tracked = true; + JointData joints[XRBodyTracker::JOINT_MAX]; + + Skeleton3D *get_skeleton(); + void _get_joint_data(); + void _update_skeleton(); + void _tracker_changed(const StringName &p_tracker_name, const Ref &p_tracker); +}; + +VARIANT_BITFIELD_CAST(XRBodyModifier3D::BodyUpdate) +VARIANT_ENUM_CAST(XRBodyModifier3D::BoneUpdate) + +#endif // XR_BODY_MODIFIER_3D_H diff --git a/scene/register_scene_types.cpp b/scene/register_scene_types.cpp index b156cd04222..826416aa9cc 100644 --- a/scene/register_scene_types.cpp +++ b/scene/register_scene_types.cpp @@ -260,6 +260,7 @@ #include "scene/3d/visible_on_screen_notifier_3d.h" #include "scene/3d/voxel_gi.h" #include "scene/3d/world_environment.h" +#include "scene/3d/xr_body_modifier_3d.h" #include "scene/3d/xr_face_modifier_3d.h" #include "scene/3d/xr_hand_modifier_3d.h" #include "scene/3d/xr_nodes.h" @@ -522,6 +523,7 @@ void register_scene_types() { GDREGISTER_CLASS(XRController3D); GDREGISTER_CLASS(XRAnchor3D); GDREGISTER_CLASS(XROrigin3D); + GDREGISTER_CLASS(XRBodyModifier3D); GDREGISTER_CLASS(XRHandModifier3D); GDREGISTER_CLASS(XRFaceModifier3D); GDREGISTER_CLASS(MeshInstance3D); diff --git a/servers/register_server_types.cpp b/servers/register_server_types.cpp index 380500769cf..ee2a65ad913 100644 --- a/servers/register_server_types.cpp +++ b/servers/register_server_types.cpp @@ -76,6 +76,7 @@ #include "text/text_server_dummy.h" #include "text/text_server_extension.h" #include "text_server.h" +#include "xr/xr_body_tracker.h" #include "xr/xr_face_tracker.h" #include "xr/xr_hand_tracker.h" #include "xr/xr_interface.h" @@ -173,6 +174,7 @@ void register_server_types() { GDREGISTER_ABSTRACT_CLASS(RenderingDevice); + GDREGISTER_CLASS(XRBodyTracker); GDREGISTER_ABSTRACT_CLASS(XRInterface); GDREGISTER_CLASS(XRHandTracker); GDREGISTER_CLASS(XRInterfaceExtension); // can't register this as virtual because we need a creation function for our extensions. diff --git a/servers/xr/xr_body_tracker.cpp b/servers/xr/xr_body_tracker.cpp new file mode 100644 index 00000000000..cd58c14348e --- /dev/null +++ b/servers/xr/xr_body_tracker.cpp @@ -0,0 +1,171 @@ +/**************************************************************************/ +/* xr_body_tracker.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "xr_body_tracker.h" + +void XRBodyTracker::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_has_tracking_data", "has_data"), &XRBodyTracker::set_has_tracking_data); + ClassDB::bind_method(D_METHOD("get_has_tracking_data"), &XRBodyTracker::get_has_tracking_data); + + ClassDB::bind_method(D_METHOD("set_body_flags", "flags"), &XRBodyTracker::set_body_flags); + ClassDB::bind_method(D_METHOD("get_body_flags"), &XRBodyTracker::get_body_flags); + + ClassDB::bind_method(D_METHOD("set_joint_flags", "joint", "flags"), &XRBodyTracker::set_joint_flags); + ClassDB::bind_method(D_METHOD("get_joint_flags", "joint"), &XRBodyTracker::get_joint_flags); + + ClassDB::bind_method(D_METHOD("set_joint_transform", "joint", "transform"), &XRBodyTracker::set_joint_transform); + ClassDB::bind_method(D_METHOD("get_joint_transform", "joint"), &XRBodyTracker::get_joint_transform); + + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "has_tracking_data", PROPERTY_HINT_NONE), "set_has_tracking_data", "get_has_tracking_data"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "body_flags", PROPERTY_HINT_FLAGS, "Upper Body,Lower Body,Hands"), "set_body_flags", "get_body_flags"); + + BIND_BITFIELD_FLAG(BODY_FLAG_UPPER_BODY_SUPPORTED); + BIND_BITFIELD_FLAG(BODY_FLAG_LOWER_BODY_SUPPORTED); + BIND_BITFIELD_FLAG(BODY_FLAG_HANDS_SUPPORTED); + + BIND_ENUM_CONSTANT(JOINT_ROOT); + BIND_ENUM_CONSTANT(JOINT_HIPS); + BIND_ENUM_CONSTANT(JOINT_SPINE); + BIND_ENUM_CONSTANT(JOINT_CHEST); + BIND_ENUM_CONSTANT(JOINT_UPPER_CHEST); + BIND_ENUM_CONSTANT(JOINT_NECK); + BIND_ENUM_CONSTANT(JOINT_HEAD); + BIND_ENUM_CONSTANT(JOINT_HEAD_TIP); + BIND_ENUM_CONSTANT(JOINT_LEFT_SHOULDER); + BIND_ENUM_CONSTANT(JOINT_LEFT_UPPER_ARM); + BIND_ENUM_CONSTANT(JOINT_LEFT_LOWER_ARM); + BIND_ENUM_CONSTANT(JOINT_RIGHT_SHOULDER); + BIND_ENUM_CONSTANT(JOINT_RIGHT_UPPER_ARM); + BIND_ENUM_CONSTANT(JOINT_RIGHT_LOWER_ARM); + BIND_ENUM_CONSTANT(JOINT_LEFT_UPPER_LEG); + BIND_ENUM_CONSTANT(JOINT_LEFT_LOWER_LEG); + BIND_ENUM_CONSTANT(JOINT_LEFT_FOOT); + BIND_ENUM_CONSTANT(JOINT_LEFT_TOES); + BIND_ENUM_CONSTANT(JOINT_RIGHT_UPPER_LEG); + BIND_ENUM_CONSTANT(JOINT_RIGHT_LOWER_LEG); + BIND_ENUM_CONSTANT(JOINT_RIGHT_FOOT); + BIND_ENUM_CONSTANT(JOINT_RIGHT_TOES); + BIND_ENUM_CONSTANT(JOINT_LEFT_HAND); + BIND_ENUM_CONSTANT(JOINT_LEFT_PALM); + BIND_ENUM_CONSTANT(JOINT_LEFT_WRIST); + BIND_ENUM_CONSTANT(JOINT_LEFT_THUMB_METACARPAL); + BIND_ENUM_CONSTANT(JOINT_LEFT_THUMB_PHALANX_PROXIMAL); + BIND_ENUM_CONSTANT(JOINT_LEFT_THUMB_PHALANX_DISTAL); + BIND_ENUM_CONSTANT(JOINT_LEFT_THUMB_TIP); + BIND_ENUM_CONSTANT(JOINT_LEFT_INDEX_FINGER_METACARPAL); + BIND_ENUM_CONSTANT(JOINT_LEFT_INDEX_FINGER_PHALANX_PROXIMAL); + BIND_ENUM_CONSTANT(JOINT_LEFT_INDEX_FINGER_PHALANX_INTERMEDIATE); + BIND_ENUM_CONSTANT(JOINT_LEFT_INDEX_FINGER_PHALANX_DISTAL); + BIND_ENUM_CONSTANT(JOINT_LEFT_INDEX_FINGER_TIP); + BIND_ENUM_CONSTANT(JOINT_LEFT_MIDDLE_FINGER_METACARPAL); + BIND_ENUM_CONSTANT(JOINT_LEFT_MIDDLE_FINGER_PHALANX_PROXIMAL); + BIND_ENUM_CONSTANT(JOINT_LEFT_MIDDLE_FINGER_PHALANX_INTERMEDIATE); + BIND_ENUM_CONSTANT(JOINT_LEFT_MIDDLE_FINGER_PHALANX_DISTAL); + BIND_ENUM_CONSTANT(JOINT_LEFT_MIDDLE_FINGER_TIP); + BIND_ENUM_CONSTANT(JOINT_LEFT_RING_FINGER_METACARPAL); + BIND_ENUM_CONSTANT(JOINT_LEFT_RING_FINGER_PHALANX_PROXIMAL); + BIND_ENUM_CONSTANT(JOINT_LEFT_RING_FINGER_PHALANX_INTERMEDIATE); + BIND_ENUM_CONSTANT(JOINT_LEFT_RING_FINGER_PHALANX_DISTAL); + BIND_ENUM_CONSTANT(JOINT_LEFT_RING_FINGER_TIP); + BIND_ENUM_CONSTANT(JOINT_LEFT_PINKY_FINGER_METACARPAL); + BIND_ENUM_CONSTANT(JOINT_LEFT_PINKY_FINGER_PHALANX_PROXIMAL); + BIND_ENUM_CONSTANT(JOINT_LEFT_PINKY_FINGER_PHALANX_INTERMEDIATE); + BIND_ENUM_CONSTANT(JOINT_LEFT_PINKY_FINGER_PHALANX_DISTAL); + BIND_ENUM_CONSTANT(JOINT_LEFT_PINKY_FINGER_TIP); + BIND_ENUM_CONSTANT(JOINT_RIGHT_HAND); + BIND_ENUM_CONSTANT(JOINT_RIGHT_PALM); + BIND_ENUM_CONSTANT(JOINT_RIGHT_WRIST); + BIND_ENUM_CONSTANT(JOINT_RIGHT_THUMB_METACARPAL); + BIND_ENUM_CONSTANT(JOINT_RIGHT_THUMB_PHALANX_PROXIMAL); + BIND_ENUM_CONSTANT(JOINT_RIGHT_THUMB_PHALANX_DISTAL); + BIND_ENUM_CONSTANT(JOINT_RIGHT_THUMB_TIP); + BIND_ENUM_CONSTANT(JOINT_RIGHT_INDEX_FINGER_METACARPAL); + BIND_ENUM_CONSTANT(JOINT_RIGHT_INDEX_FINGER_PHALANX_PROXIMAL); + BIND_ENUM_CONSTANT(JOINT_RIGHT_INDEX_FINGER_PHALANX_INTERMEDIATE); + BIND_ENUM_CONSTANT(JOINT_RIGHT_INDEX_FINGER_PHALANX_DISTAL); + BIND_ENUM_CONSTANT(JOINT_RIGHT_INDEX_FINGER_TIP); + BIND_ENUM_CONSTANT(JOINT_RIGHT_MIDDLE_FINGER_METACARPAL); + BIND_ENUM_CONSTANT(JOINT_RIGHT_MIDDLE_FINGER_PHALANX_PROXIMAL); + BIND_ENUM_CONSTANT(JOINT_RIGHT_MIDDLE_FINGER_PHALANX_INTERMEDIATE); + BIND_ENUM_CONSTANT(JOINT_RIGHT_MIDDLE_FINGER_PHALANX_DISTAL); + BIND_ENUM_CONSTANT(JOINT_RIGHT_MIDDLE_FINGER_TIP); + BIND_ENUM_CONSTANT(JOINT_RIGHT_RING_FINGER_METACARPAL); + BIND_ENUM_CONSTANT(JOINT_RIGHT_RING_FINGER_PHALANX_PROXIMAL); + BIND_ENUM_CONSTANT(JOINT_RIGHT_RING_FINGER_PHALANX_INTERMEDIATE); + BIND_ENUM_CONSTANT(JOINT_RIGHT_RING_FINGER_PHALANX_DISTAL); + BIND_ENUM_CONSTANT(JOINT_RIGHT_RING_FINGER_TIP); + BIND_ENUM_CONSTANT(JOINT_RIGHT_PINKY_FINGER_METACARPAL); + BIND_ENUM_CONSTANT(JOINT_RIGHT_PINKY_FINGER_PHALANX_PROXIMAL); + BIND_ENUM_CONSTANT(JOINT_RIGHT_PINKY_FINGER_PHALANX_INTERMEDIATE); + BIND_ENUM_CONSTANT(JOINT_RIGHT_PINKY_FINGER_PHALANX_DISTAL); + BIND_ENUM_CONSTANT(JOINT_RIGHT_PINKY_FINGER_TIP); + BIND_ENUM_CONSTANT(JOINT_MAX); + + BIND_BITFIELD_FLAG(JOINT_FLAG_ORIENTATION_VALID); + BIND_BITFIELD_FLAG(JOINT_FLAG_ORIENTATION_TRACKED); + BIND_BITFIELD_FLAG(JOINT_FLAG_POSITION_VALID); + BIND_BITFIELD_FLAG(JOINT_FLAG_POSITION_TRACKED); +} + +void XRBodyTracker::set_has_tracking_data(bool p_has_tracking_data) { + has_tracking_data = p_has_tracking_data; +} + +bool XRBodyTracker::get_has_tracking_data() const { + return has_tracking_data; +} + +void XRBodyTracker::set_body_flags(BitField p_body_flags) { + body_flags = p_body_flags; +} + +BitField XRBodyTracker::get_body_flags() const { + return body_flags; +} + +void XRBodyTracker::set_joint_flags(Joint p_joint, BitField p_flags) { + ERR_FAIL_INDEX(p_joint, JOINT_MAX); + joint_flags[p_joint] = p_flags; +} + +BitField XRBodyTracker::get_joint_flags(Joint p_joint) const { + ERR_FAIL_INDEX_V(p_joint, JOINT_MAX, BitField()); + return joint_flags[p_joint]; +} + +void XRBodyTracker::set_joint_transform(Joint p_joint, const Transform3D &p_transform) { + ERR_FAIL_INDEX(p_joint, JOINT_MAX); + joint_transforms[p_joint] = p_transform; +} + +Transform3D XRBodyTracker::get_joint_transform(Joint p_joint) const { + ERR_FAIL_INDEX_V(p_joint, JOINT_MAX, Transform3D()); + return joint_transforms[p_joint]; +} diff --git a/servers/xr/xr_body_tracker.h b/servers/xr/xr_body_tracker.h new file mode 100644 index 00000000000..659aa39df1b --- /dev/null +++ b/servers/xr/xr_body_tracker.h @@ -0,0 +1,170 @@ +/**************************************************************************/ +/* xr_body_tracker.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef XR_BODY_TRACKER_H +#define XR_BODY_TRACKER_H + +#include "core/object/ref_counted.h" + +class XRBodyTracker : public RefCounted { + GDCLASS(XRBodyTracker, RefCounted); + _THREAD_SAFE_CLASS_ + +public: + enum BodyFlags { + BODY_FLAG_UPPER_BODY_SUPPORTED = 1, + BODY_FLAG_LOWER_BODY_SUPPORTED = 2, + BODY_FLAG_HANDS_SUPPORTED = 4, + }; + + enum Joint { + JOINT_ROOT, + + // Upper Body Joints + JOINT_HIPS, + JOINT_SPINE, + JOINT_CHEST, + JOINT_UPPER_CHEST, + JOINT_NECK, + JOINT_HEAD, + JOINT_HEAD_TIP, + JOINT_LEFT_SHOULDER, + JOINT_LEFT_UPPER_ARM, + JOINT_LEFT_LOWER_ARM, + JOINT_RIGHT_SHOULDER, + JOINT_RIGHT_UPPER_ARM, + JOINT_RIGHT_LOWER_ARM, + + // Lower Body Joints + JOINT_LEFT_UPPER_LEG, + JOINT_LEFT_LOWER_LEG, + JOINT_LEFT_FOOT, + JOINT_LEFT_TOES, + JOINT_RIGHT_UPPER_LEG, + JOINT_RIGHT_LOWER_LEG, + JOINT_RIGHT_FOOT, + JOINT_RIGHT_TOES, + + // Left Hand Joints + JOINT_LEFT_HAND, + JOINT_LEFT_PALM, + JOINT_LEFT_WRIST, + JOINT_LEFT_THUMB_METACARPAL, + JOINT_LEFT_THUMB_PHALANX_PROXIMAL, + JOINT_LEFT_THUMB_PHALANX_DISTAL, + JOINT_LEFT_THUMB_TIP, + JOINT_LEFT_INDEX_FINGER_METACARPAL, + JOINT_LEFT_INDEX_FINGER_PHALANX_PROXIMAL, + JOINT_LEFT_INDEX_FINGER_PHALANX_INTERMEDIATE, + JOINT_LEFT_INDEX_FINGER_PHALANX_DISTAL, + JOINT_LEFT_INDEX_FINGER_TIP, + JOINT_LEFT_MIDDLE_FINGER_METACARPAL, + JOINT_LEFT_MIDDLE_FINGER_PHALANX_PROXIMAL, + JOINT_LEFT_MIDDLE_FINGER_PHALANX_INTERMEDIATE, + JOINT_LEFT_MIDDLE_FINGER_PHALANX_DISTAL, + JOINT_LEFT_MIDDLE_FINGER_TIP, + JOINT_LEFT_RING_FINGER_METACARPAL, + JOINT_LEFT_RING_FINGER_PHALANX_PROXIMAL, + JOINT_LEFT_RING_FINGER_PHALANX_INTERMEDIATE, + JOINT_LEFT_RING_FINGER_PHALANX_DISTAL, + JOINT_LEFT_RING_FINGER_TIP, + JOINT_LEFT_PINKY_FINGER_METACARPAL, + JOINT_LEFT_PINKY_FINGER_PHALANX_PROXIMAL, + JOINT_LEFT_PINKY_FINGER_PHALANX_INTERMEDIATE, + JOINT_LEFT_PINKY_FINGER_PHALANX_DISTAL, + JOINT_LEFT_PINKY_FINGER_TIP, + + // Right Hand Joints + JOINT_RIGHT_HAND, + JOINT_RIGHT_PALM, + JOINT_RIGHT_WRIST, + JOINT_RIGHT_THUMB_METACARPAL, + JOINT_RIGHT_THUMB_PHALANX_PROXIMAL, + JOINT_RIGHT_THUMB_PHALANX_DISTAL, + JOINT_RIGHT_THUMB_TIP, + JOINT_RIGHT_INDEX_FINGER_METACARPAL, + JOINT_RIGHT_INDEX_FINGER_PHALANX_PROXIMAL, + JOINT_RIGHT_INDEX_FINGER_PHALANX_INTERMEDIATE, + JOINT_RIGHT_INDEX_FINGER_PHALANX_DISTAL, + JOINT_RIGHT_INDEX_FINGER_TIP, + JOINT_RIGHT_MIDDLE_FINGER_METACARPAL, + JOINT_RIGHT_MIDDLE_FINGER_PHALANX_PROXIMAL, + JOINT_RIGHT_MIDDLE_FINGER_PHALANX_INTERMEDIATE, + JOINT_RIGHT_MIDDLE_FINGER_PHALANX_DISTAL, + JOINT_RIGHT_MIDDLE_FINGER_TIP, + JOINT_RIGHT_RING_FINGER_METACARPAL, + JOINT_RIGHT_RING_FINGER_PHALANX_PROXIMAL, + JOINT_RIGHT_RING_FINGER_PHALANX_INTERMEDIATE, + JOINT_RIGHT_RING_FINGER_PHALANX_DISTAL, + JOINT_RIGHT_RING_FINGER_TIP, + JOINT_RIGHT_PINKY_FINGER_METACARPAL, + JOINT_RIGHT_PINKY_FINGER_PHALANX_PROXIMAL, + JOINT_RIGHT_PINKY_FINGER_PHALANX_INTERMEDIATE, + JOINT_RIGHT_PINKY_FINGER_PHALANX_DISTAL, + JOINT_RIGHT_PINKY_FINGER_TIP, + + JOINT_MAX, + }; + + enum JointFlags { + JOINT_FLAG_ORIENTATION_VALID = 1, + JOINT_FLAG_ORIENTATION_TRACKED = 2, + JOINT_FLAG_POSITION_VALID = 4, + JOINT_FLAG_POSITION_TRACKED = 8, + }; + + void set_has_tracking_data(bool p_has_tracking_data); + bool get_has_tracking_data() const; + + void set_body_flags(BitField p_body_flags); + BitField get_body_flags() const; + + void set_joint_flags(Joint p_joint, BitField p_flags); + BitField get_joint_flags(Joint p_joint) const; + + void set_joint_transform(Joint p_joint, const Transform3D &p_transform); + Transform3D get_joint_transform(Joint p_joint) const; + +protected: + static void _bind_methods(); + +private: + bool has_tracking_data = false; + BitField body_flags; + + BitField joint_flags[JOINT_MAX]; + Transform3D joint_transforms[JOINT_MAX]; +}; + +VARIANT_BITFIELD_CAST(XRBodyTracker::BodyFlags) +VARIANT_ENUM_CAST(XRBodyTracker::Joint) +VARIANT_BITFIELD_CAST(XRBodyTracker::JointFlags) + +#endif // XR_BODY_TRACKER_H diff --git a/servers/xr_server.cpp b/servers/xr_server.cpp index 1e497e22c3b..af14ba4a00b 100644 --- a/servers/xr_server.cpp +++ b/servers/xr_server.cpp @@ -30,6 +30,7 @@ #include "xr_server.h" #include "core/config/project_settings.h" +#include "xr/xr_body_tracker.h" #include "xr/xr_face_tracker.h" #include "xr/xr_hand_tracker.h" #include "xr/xr_interface.h" @@ -86,6 +87,11 @@ void XRServer::_bind_methods() { ClassDB::bind_method(D_METHOD("get_face_trackers"), &XRServer::get_face_trackers); ClassDB::bind_method(D_METHOD("get_face_tracker", "tracker_name"), &XRServer::get_face_tracker); + ClassDB::bind_method(D_METHOD("add_body_tracker", "tracker_name", "body_tracker"), &XRServer::add_body_tracker); + ClassDB::bind_method(D_METHOD("remove_body_tracker", "tracker_name"), &XRServer::remove_body_tracker); + ClassDB::bind_method(D_METHOD("get_body_trackers"), &XRServer::get_body_trackers); + ClassDB::bind_method(D_METHOD("get_body_tracker", "tracker_name"), &XRServer::get_body_tracker); + ClassDB::bind_method(D_METHOD("get_primary_interface"), &XRServer::get_primary_interface); ClassDB::bind_method(D_METHOD("set_primary_interface", "interface"), &XRServer::set_primary_interface); @@ -117,6 +123,10 @@ void XRServer::_bind_methods() { ADD_SIGNAL(MethodInfo("face_tracker_added", PropertyInfo(Variant::STRING_NAME, "tracker_name"), PropertyInfo(Variant::OBJECT, "face_tracker", PROPERTY_HINT_RESOURCE_TYPE, "XRFaceTracker"))); ADD_SIGNAL(MethodInfo("face_tracker_updated", PropertyInfo(Variant::STRING_NAME, "tracker_name"), PropertyInfo(Variant::OBJECT, "face_tracker", PROPERTY_HINT_RESOURCE_TYPE, "XRFaceTracker"))); ADD_SIGNAL(MethodInfo("face_tracker_removed", PropertyInfo(Variant::STRING_NAME, "tracker_name"))); + + ADD_SIGNAL(MethodInfo("body_tracker_added", PropertyInfo(Variant::STRING_NAME, "tracker_name"), PropertyInfo(Variant::OBJECT, "body_tracker", PROPERTY_HINT_RESOURCE_TYPE, "XRBodyTracker"))); + ADD_SIGNAL(MethodInfo("body_tracker_updated", PropertyInfo(Variant::STRING_NAME, "tracker_name"), PropertyInfo(Variant::OBJECT, "body_tracker", PROPERTY_HINT_RESOURCE_TYPE, "XRBodyTracker"))); + ADD_SIGNAL(MethodInfo("body_tracker_removed", PropertyInfo(Variant::STRING_NAME, "tracker_name"))); }; double XRServer::get_world_scale() const { @@ -448,6 +458,44 @@ Ref XRServer::get_face_tracker(const StringName &p_tracker_name) return face_trackers[p_tracker_name]; } +void XRServer::add_body_tracker(const StringName &p_tracker_name, Ref p_body_tracker) { + ERR_FAIL_COND(p_body_tracker.is_null()); + + if (!body_trackers.has(p_tracker_name)) { + // We don't have a tracker with this name, we're going to add it. + body_trackers[p_tracker_name] = p_body_tracker; + emit_signal(SNAME("body_tracker_added"), p_tracker_name, p_body_tracker); + } else if (body_trackers[p_tracker_name] != p_body_tracker) { + // We already have a tracker with this name, we're going to replace it. + body_trackers[p_tracker_name] = p_body_tracker; + emit_signal(SNAME("body_tracker_updated"), p_tracker_name, p_body_tracker); + } +} + +void XRServer::remove_body_tracker(const StringName &p_tracker_name) { + // Skip if no face tracker is found. + if (!body_trackers.has(p_tracker_name)) { + return; + } + + // Send the removed signal, then remove the face tracker. + emit_signal(SNAME("body_tracker_removed"), p_tracker_name); + body_trackers.erase(p_tracker_name); +} + +Dictionary XRServer::get_body_trackers() const { + return body_trackers; +} + +Ref XRServer::get_body_tracker(const StringName &p_tracker_name) const { + // Skip if no tracker is found. + if (!body_trackers.has(p_tracker_name)) { + return Ref(); + } + + return body_trackers[p_tracker_name]; +} + void XRServer::_process() { // called from our main game loop before we handle physics and game logic // note that we can have multiple interfaces active if we have interfaces that purely handle tracking diff --git a/servers/xr_server.h b/servers/xr_server.h index 3e45bbb76c9..6aaa34b21dc 100644 --- a/servers/xr_server.h +++ b/servers/xr_server.h @@ -41,6 +41,7 @@ class XRInterface; class XRPositionalTracker; class XRHandTracker; class XRFaceTracker; +class XRBodyTracker; /** The XR server is a singleton object that gives access to the various @@ -89,6 +90,7 @@ private: Dictionary trackers; Dictionary hand_trackers; Dictionary face_trackers; + Dictionary body_trackers; Ref primary_interface; /* we'll identify one interface as primary, this will be used by our viewports */ @@ -203,6 +205,14 @@ public: Dictionary get_face_trackers() const; Ref get_face_tracker(const StringName &p_tracker_name) const; + /* + Body trackers are objects that expose the tracked joints of a body. + */ + void add_body_tracker(const StringName &p_tracker_name, Ref p_face_tracker); + void remove_body_tracker(const StringName &p_tracker_name); + Dictionary get_body_trackers() const; + Ref get_body_tracker(const StringName &p_tracker_name) const; + // Process is called before we handle our physics process and game process. This is where our interfaces will update controller data and such. void _process();