diff options
Diffstat (limited to 'modules/openxr/scene/openxr_hand.cpp')
| -rw-r--r-- | modules/openxr/scene/openxr_hand.cpp | 307 |
1 files changed, 307 insertions, 0 deletions
diff --git a/modules/openxr/scene/openxr_hand.cpp b/modules/openxr/scene/openxr_hand.cpp new file mode 100644 index 0000000000..2ae13a1026 --- /dev/null +++ b/modules/openxr/scene/openxr_hand.cpp @@ -0,0 +1,307 @@ +/*************************************************************************/ +/* openxr_hand.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* 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 "../extensions/openxr_hand_tracking_extension.h" +#include "../openxr_api.h" + +#include "openxr_hand.h" +#include "scene/3d/skeleton_3d.h" +#include "servers/xr_server.h" + +void OpenXRHand::_bind_methods() { + ClassDB::bind_method(D_METHOD("set_hand", "hand"), &OpenXRHand::set_hand); + ClassDB::bind_method(D_METHOD("get_hand"), &OpenXRHand::get_hand); + + ClassDB::bind_method(D_METHOD("set_hand_skeleton", "hand_skeleton"), &OpenXRHand::set_hand_skeleton); + ClassDB::bind_method(D_METHOD("get_hand_skeleton"), &OpenXRHand::get_hand_skeleton); + + 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); + + 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"); + + BIND_ENUM_CONSTANT(HAND_LEFT); + BIND_ENUM_CONSTANT(HAND_RIGHT); + BIND_ENUM_CONSTANT(HAND_MAX); + + BIND_ENUM_CONSTANT(MOTION_RANGE_UNOBSTRUCTED); + BIND_ENUM_CONSTANT(MOTION_RANGE_CONFORM_TO_CONTROLLER); + BIND_ENUM_CONSTANT(MOTION_RANGE_MAX); +} + +OpenXRHand::OpenXRHand() { + openxr_api = OpenXRAPI::get_singleton(); + hand_tracking_ext = OpenXRHandTrackingExtension::get_singleton(); +} + +void OpenXRHand::set_hand(const Hands p_hand) { + ERR_FAIL_INDEX(p_hand, HAND_MAX); + + hand = p_hand; +} + +OpenXRHand::Hands OpenXRHand::get_hand() const { + return hand; +} + +void OpenXRHand::set_hand_skeleton(const NodePath &p_hand_skeleton) { + hand_skeleton = p_hand_skeleton; + + // TODO if inside tree call _get_bones() +} + +void OpenXRHand::set_motion_range(const MotionRange p_motion_range) { + ERR_FAIL_INDEX(p_motion_range, MOTION_RANGE_MAX); + motion_range = p_motion_range; + + _set_motion_range(); +} + +OpenXRHand::MotionRange OpenXRHand::get_motion_range() const { + return motion_range; +} + +NodePath OpenXRHand::get_hand_skeleton() const { + return hand_skeleton; +} + +void OpenXRHand::_set_motion_range() { + if (!hand_tracking_ext) { + return; + } + + XrHandJointsMotionRangeEXT xr_motion_range; + switch (motion_range) { + case MOTION_RANGE_UNOBSTRUCTED: + xr_motion_range = XR_HAND_JOINTS_MOTION_RANGE_UNOBSTRUCTED_EXT; + break; + case MOTION_RANGE_CONFORM_TO_CONTROLLER: + xr_motion_range = XR_HAND_JOINTS_MOTION_RANGE_CONFORMING_TO_CONTROLLER_EXT; + break; + default: + xr_motion_range = XR_HAND_JOINTS_MOTION_RANGE_CONFORMING_TO_CONTROLLER_EXT; + break; + } + + hand_tracking_ext->set_motion_range(hand, xr_motion_range); +} + +Skeleton3D *OpenXRHand::get_skeleton() { + if (!has_node(hand_skeleton)) { + return nullptr; + } + + Node *node = get_node(hand_skeleton); + if (!node) { + return nullptr; + } + + Skeleton3D *skeleton = Object::cast_to<Skeleton3D>(node); + 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", + }; + + // reset JIC + for (int i = 0; i < XR_HAND_JOINT_COUNT_EXT; i++) { + bones[i] = -1; + } + + Skeleton3D *skeleton = get_skeleton(); + if (!skeleton) { + return; + } + + // We cast to spatials which should allow us to use any subclass of that. + 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"); + } + + bones[i] = skeleton->find_bone(bone_name); + if (bones[i] == -1) { + print_line("Couldn't obtain bone for", bone_name); + } + } +} + +void OpenXRHand::_update_skeleton() { + if (openxr_api == nullptr || !openxr_api->is_initialized()) { + return; + } else if (hand_tracking_ext == nullptr || !hand_tracking_ext->get_active()) { + return; + } + + Skeleton3D *skeleton = get_skeleton(); + if (!skeleton) { + return; + } + + // 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 OpenXRHandTrackingExtension::HandTracker *hand_tracker = hand_tracking_ext->get_hand_tracker(hand); + const float ws = XRServer::get_singleton()->get_world_scale(); + + if (hand_tracker->is_initialised && hand_tracker->locations.isActive) { + for (int i = 0; i < XR_HAND_JOINT_COUNT_EXT; i++) { + confidences[i] = XRPose::XR_TRACKING_CONFIDENCE_NONE; + quaternions[i] = Quaternion(); + positions[i] = Vector3(); + + const auto &location = hand_tracker->joint_locations[i]; + const auto &pose = location.pose; + + if (location.locationFlags & XR_SPACE_LOCATION_ORIENTATION_VALID_BIT) { + if (pose.orientation.x != 0 || pose.orientation.y != 0 || pose.orientation.y != 0 || pose.orientation.w != 0) { + quaternions[i] = Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); + inv_quaternions[i] = quaternions[i].inverse(); + + if (location.locationFlags & XR_SPACE_LOCATION_POSITION_VALID_BIT) { + confidences[i] = XRPose::XR_TRACKING_CONFIDENCE_HIGH; + positions[i] = Vector3(pose.position.x * ws, pose.position.y * ws, pose.position.z * ws); + + // TODO get inverse of position, we'll do this later. For now we're ignoring bone positions which generally works better anyway + } else { + confidences[i] = XRPose::XR_TRACKING_CONFIDENCE_LOW; + } + } + } + } + + 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]; + + // 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; + } 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; + found = true; + } + } + } + + // And get the movement from our rest position + // Transform3D rest = skeleton->get_bone_rest(bones[i]); + // q = rest.basis.get_quaternion().inverse() * q; + + // and set our pose + // skeleton->set_bone_pose_position(bones[i], v); + skeleton->set_bone_pose_rotation(bones[i], q); + } + } + + Transform3D t; + t.basis = Basis(quaternions[XR_HAND_JOINT_PALM_EXT]); + t.origin = positions[XR_HAND_JOINT_PALM_EXT]; + set_transform(t); + + // show it + set_visible(true); + } else { + // hide it + set_visible(false); + } + } else { + // hide it + set_visible(false); + } +} + +void OpenXRHand::_notification(int p_what) { + switch (p_what) { + case NOTIFICATION_ENTER_TREE: { + _get_bones(); + + set_process_internal(true); + } break; + case NOTIFICATION_EXIT_TREE: { + set_process_internal(false); + + // reset + for (int i = 0; i < XR_HAND_JOINT_COUNT_EXT; i++) { + bones[i] = -1; + } + } break; + case NOTIFICATION_INTERNAL_PROCESS: { + _update_skeleton(); + } break; + default: { + } break; + } +} |