1/**************************************************************************/
2/* openxr_hand.cpp */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10/* */
11/* Permission is hereby granted, free of charge, to any person obtaining */
12/* a copy of this software and associated documentation files (the */
13/* "Software"), to deal in the Software without restriction, including */
14/* without limitation the rights to use, copy, modify, merge, publish, */
15/* distribute, sublicense, and/or sell copies of the Software, and to */
16/* permit persons to whom the Software is furnished to do so, subject to */
17/* the following conditions: */
18/* */
19/* The above copyright notice and this permission notice shall be */
20/* included in all copies or substantial portions of the Software. */
21/* */
22/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29/**************************************************************************/
30
31#include "openxr_hand.h"
32
33#include "../extensions/openxr_hand_tracking_extension.h"
34#include "../openxr_api.h"
35
36#include "scene/3d/skeleton_3d.h"
37#include "servers/xr_server.h"
38
39void OpenXRHand::_bind_methods() {
40 ClassDB::bind_method(D_METHOD("set_hand", "hand"), &OpenXRHand::set_hand);
41 ClassDB::bind_method(D_METHOD("get_hand"), &OpenXRHand::get_hand);
42
43 ClassDB::bind_method(D_METHOD("set_hand_skeleton", "hand_skeleton"), &OpenXRHand::set_hand_skeleton);
44 ClassDB::bind_method(D_METHOD("get_hand_skeleton"), &OpenXRHand::get_hand_skeleton);
45
46 ClassDB::bind_method(D_METHOD("set_motion_range", "motion_range"), &OpenXRHand::set_motion_range);
47 ClassDB::bind_method(D_METHOD("get_motion_range"), &OpenXRHand::get_motion_range);
48
49 ADD_PROPERTY(PropertyInfo(Variant::INT, "hand", PROPERTY_HINT_ENUM, "Left,Right"), "set_hand", "get_hand");
50 ADD_PROPERTY(PropertyInfo(Variant::INT, "motion_range", PROPERTY_HINT_ENUM, "Unobstructed,Conform to controller"), "set_motion_range", "get_motion_range");
51 ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "hand_skeleton", PROPERTY_HINT_NODE_PATH_VALID_TYPES, "Skeleton3D"), "set_hand_skeleton", "get_hand_skeleton");
52
53 BIND_ENUM_CONSTANT(HAND_LEFT);
54 BIND_ENUM_CONSTANT(HAND_RIGHT);
55 BIND_ENUM_CONSTANT(HAND_MAX);
56
57 BIND_ENUM_CONSTANT(MOTION_RANGE_UNOBSTRUCTED);
58 BIND_ENUM_CONSTANT(MOTION_RANGE_CONFORM_TO_CONTROLLER);
59 BIND_ENUM_CONSTANT(MOTION_RANGE_MAX);
60}
61
62OpenXRHand::OpenXRHand() {
63 openxr_api = OpenXRAPI::get_singleton();
64 hand_tracking_ext = OpenXRHandTrackingExtension::get_singleton();
65}
66
67void OpenXRHand::set_hand(const Hands p_hand) {
68 ERR_FAIL_INDEX(p_hand, HAND_MAX);
69
70 hand = p_hand;
71}
72
73OpenXRHand::Hands OpenXRHand::get_hand() const {
74 return hand;
75}
76
77void OpenXRHand::set_hand_skeleton(const NodePath &p_hand_skeleton) {
78 hand_skeleton = p_hand_skeleton;
79
80 // TODO if inside tree call _get_bones()
81}
82
83void OpenXRHand::set_motion_range(const MotionRange p_motion_range) {
84 ERR_FAIL_INDEX(p_motion_range, MOTION_RANGE_MAX);
85 motion_range = p_motion_range;
86
87 _set_motion_range();
88}
89
90OpenXRHand::MotionRange OpenXRHand::get_motion_range() const {
91 return motion_range;
92}
93
94NodePath OpenXRHand::get_hand_skeleton() const {
95 return hand_skeleton;
96}
97
98void OpenXRHand::_set_motion_range() {
99 if (!hand_tracking_ext) {
100 return;
101 }
102
103 XrHandJointsMotionRangeEXT xr_motion_range;
104 switch (motion_range) {
105 case MOTION_RANGE_UNOBSTRUCTED:
106 xr_motion_range = XR_HAND_JOINTS_MOTION_RANGE_UNOBSTRUCTED_EXT;
107 break;
108 case MOTION_RANGE_CONFORM_TO_CONTROLLER:
109 xr_motion_range = XR_HAND_JOINTS_MOTION_RANGE_CONFORMING_TO_CONTROLLER_EXT;
110 break;
111 default:
112 xr_motion_range = XR_HAND_JOINTS_MOTION_RANGE_CONFORMING_TO_CONTROLLER_EXT;
113 break;
114 }
115
116 hand_tracking_ext->set_motion_range(hand, xr_motion_range);
117}
118
119Skeleton3D *OpenXRHand::get_skeleton() {
120 if (!has_node(hand_skeleton)) {
121 return nullptr;
122 }
123
124 Node *node = get_node(hand_skeleton);
125 if (!node) {
126 return nullptr;
127 }
128
129 Skeleton3D *skeleton = Object::cast_to<Skeleton3D>(node);
130 return skeleton;
131}
132
133void OpenXRHand::_get_bones() {
134 const char *bone_names[XR_HAND_JOINT_COUNT_EXT] = {
135 "Palm",
136 "Wrist",
137 "Thumb_Metacarpal",
138 "Thumb_Proximal",
139 "Thumb_Distal",
140 "Thumb_Tip",
141 "Index_Metacarpal",
142 "Index_Proximal",
143 "Index_Intermediate",
144 "Index_Distal",
145 "Index_Tip",
146 "Middle_Metacarpal",
147 "Middle_Proximal",
148 "Middle_Intermediate",
149 "Middle_Distal",
150 "Middle_Tip",
151 "Ring_Metacarpal",
152 "Ring_Proximal",
153 "Ring_Intermediate",
154 "Ring_Distal",
155 "Ring_Tip",
156 "Little_Metacarpal",
157 "Little_Proximal",
158 "Little_Intermediate",
159 "Little_Distal",
160 "Little_Tip",
161 };
162
163 // reset JIC
164 for (int i = 0; i < XR_HAND_JOINT_COUNT_EXT; i++) {
165 bones[i] = -1;
166 }
167
168 Skeleton3D *skeleton = get_skeleton();
169 if (!skeleton) {
170 return;
171 }
172
173 // We cast to spatials which should allow us to use any subclass of that.
174 for (int i = 0; i < XR_HAND_JOINT_COUNT_EXT; i++) {
175 String bone_name = bone_names[i];
176 if (hand == 0) {
177 bone_name += String("_L");
178 } else {
179 bone_name += String("_R");
180 }
181
182 bones[i] = skeleton->find_bone(bone_name);
183 if (bones[i] == -1) {
184 print_line("Couldn't obtain bone for", bone_name);
185 }
186 }
187}
188
189void OpenXRHand::_update_skeleton() {
190 if (openxr_api == nullptr || !openxr_api->is_initialized()) {
191 return;
192 } else if (hand_tracking_ext == nullptr || !hand_tracking_ext->get_active()) {
193 return;
194 }
195
196 Skeleton3D *skeleton = get_skeleton();
197 if (!skeleton) {
198 return;
199 }
200
201 // we cache our transforms so we can quickly calculate local transforms
202 XRPose::TrackingConfidence confidences[XR_HAND_JOINT_COUNT_EXT];
203 Quaternion quaternions[XR_HAND_JOINT_COUNT_EXT];
204 Quaternion inv_quaternions[XR_HAND_JOINT_COUNT_EXT];
205 Vector3 positions[XR_HAND_JOINT_COUNT_EXT];
206
207 const OpenXRHandTrackingExtension::HandTracker *hand_tracker = hand_tracking_ext->get_hand_tracker(hand);
208 const float ws = XRServer::get_singleton()->get_world_scale();
209
210 if (hand_tracker->is_initialized && hand_tracker->locations.isActive) {
211 for (int i = 0; i < XR_HAND_JOINT_COUNT_EXT; i++) {
212 confidences[i] = XRPose::XR_TRACKING_CONFIDENCE_NONE;
213 quaternions[i] = Quaternion();
214 positions[i] = Vector3();
215
216 const XrHandJointLocationEXT &location = hand_tracker->joint_locations[i];
217 const XrPosef &pose = location.pose;
218
219 if (location.locationFlags & XR_SPACE_LOCATION_ORIENTATION_VALID_BIT) {
220 if (pose.orientation.x != 0 || pose.orientation.y != 0 || pose.orientation.z != 0 || pose.orientation.w != 0) {
221 quaternions[i] = Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
222 inv_quaternions[i] = quaternions[i].inverse();
223
224 if (location.locationFlags & XR_SPACE_LOCATION_POSITION_VALID_BIT) {
225 confidences[i] = XRPose::XR_TRACKING_CONFIDENCE_HIGH;
226 positions[i] = Vector3(pose.position.x * ws, pose.position.y * ws, pose.position.z * ws);
227
228 // TODO get inverse of position, we'll do this later. For now we're ignoring bone positions which generally works better anyway
229 } else {
230 confidences[i] = XRPose::XR_TRACKING_CONFIDENCE_LOW;
231 }
232 }
233 }
234 }
235
236 if (confidences[XR_HAND_JOINT_PALM_EXT] != XRPose::XR_TRACKING_CONFIDENCE_NONE) {
237 // now update our skeleton
238 for (int i = 0; i < XR_HAND_JOINT_COUNT_EXT; i++) {
239 if (bones[i] != -1) {
240 int bone = bones[i];
241 int parent = skeleton->get_bone_parent(bone);
242
243 // Get our target quaternion
244 Quaternion q = quaternions[i];
245
246 // get local translation, parent should already be processed
247 if (parent == -1) {
248 // use our palm location here, that is what we are tracking
249 q = inv_quaternions[XR_HAND_JOINT_PALM_EXT] * q;
250 } else {
251 int found = false;
252 for (int b = 0; b < XR_HAND_JOINT_COUNT_EXT && !found; b++) {
253 if (bones[b] == parent) {
254 q = inv_quaternions[b] * q;
255 found = true;
256 }
257 }
258 }
259
260 // And get the movement from our rest position
261 // Transform3D rest = skeleton->get_bone_rest(bones[i]);
262 // q = rest.basis.get_quaternion().inverse() * q;
263
264 // and set our pose
265 // skeleton->set_bone_pose_position(bones[i], v);
266 skeleton->set_bone_pose_rotation(bones[i], q);
267 }
268 }
269
270 Transform3D t;
271 t.basis = Basis(quaternions[XR_HAND_JOINT_PALM_EXT]);
272 t.origin = positions[XR_HAND_JOINT_PALM_EXT];
273 set_transform(t);
274
275 // show it
276 set_visible(true);
277 } else {
278 // hide it
279 set_visible(false);
280 }
281 } else {
282 // hide it
283 set_visible(false);
284 }
285}
286
287void OpenXRHand::_notification(int p_what) {
288 switch (p_what) {
289 case NOTIFICATION_ENTER_TREE: {
290 _get_bones();
291
292 set_process_internal(true);
293 } break;
294 case NOTIFICATION_EXIT_TREE: {
295 set_process_internal(false);
296
297 // reset
298 for (int i = 0; i < XR_HAND_JOINT_COUNT_EXT; i++) {
299 bones[i] = -1;
300 }
301 } break;
302 case NOTIFICATION_INTERNAL_PROCESS: {
303 _update_skeleton();
304 } break;
305 default: {
306 } break;
307 }
308}
309