1/**************************************************************************/
2/* openxr_hand_tracking_extension.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_tracking_extension.h"
32
33#include "../openxr_api.h"
34
35#include "core/string/print_string.h"
36#include "servers/xr_server.h"
37
38#include <openxr/openxr.h>
39
40OpenXRHandTrackingExtension *OpenXRHandTrackingExtension::singleton = nullptr;
41
42OpenXRHandTrackingExtension *OpenXRHandTrackingExtension::get_singleton() {
43 return singleton;
44}
45
46OpenXRHandTrackingExtension::OpenXRHandTrackingExtension() {
47 singleton = this;
48
49 // Make sure this is cleared until we actually request it
50 handTrackingSystemProperties.supportsHandTracking = false;
51}
52
53OpenXRHandTrackingExtension::~OpenXRHandTrackingExtension() {
54 singleton = nullptr;
55}
56
57HashMap<String, bool *> OpenXRHandTrackingExtension::get_requested_extensions() {
58 HashMap<String, bool *> request_extensions;
59
60 request_extensions[XR_EXT_HAND_TRACKING_EXTENSION_NAME] = &hand_tracking_ext;
61 request_extensions[XR_EXT_HAND_JOINTS_MOTION_RANGE_EXTENSION_NAME] = &hand_motion_range_ext;
62 request_extensions[XR_FB_HAND_TRACKING_AIM_EXTENSION_NAME] = &hand_tracking_aim_state_ext;
63
64 return request_extensions;
65}
66
67void OpenXRHandTrackingExtension::on_instance_created(const XrInstance p_instance) {
68 if (hand_tracking_ext) {
69 EXT_INIT_XR_FUNC(xrCreateHandTrackerEXT);
70 EXT_INIT_XR_FUNC(xrDestroyHandTrackerEXT);
71 EXT_INIT_XR_FUNC(xrLocateHandJointsEXT);
72
73 hand_tracking_ext = xrCreateHandTrackerEXT_ptr && xrDestroyHandTrackerEXT_ptr && xrLocateHandJointsEXT_ptr;
74 }
75}
76
77void OpenXRHandTrackingExtension::on_session_destroyed() {
78 cleanup_hand_tracking();
79}
80
81void OpenXRHandTrackingExtension::on_instance_destroyed() {
82 xrCreateHandTrackerEXT_ptr = nullptr;
83 xrDestroyHandTrackerEXT_ptr = nullptr;
84 xrLocateHandJointsEXT_ptr = nullptr;
85}
86
87void *OpenXRHandTrackingExtension::set_system_properties_and_get_next_pointer(void *p_next_pointer) {
88 if (!hand_tracking_ext) {
89 // not supported...
90 return p_next_pointer;
91 }
92
93 handTrackingSystemProperties = {
94 XR_TYPE_SYSTEM_HAND_TRACKING_PROPERTIES_EXT, // type
95 p_next_pointer, // next
96 false, // supportsHandTracking
97 };
98
99 return &handTrackingSystemProperties;
100}
101
102void OpenXRHandTrackingExtension::on_state_ready() {
103 if (!handTrackingSystemProperties.supportsHandTracking) {
104 // not supported...
105 return;
106 }
107
108 // Setup our hands and reset data
109 for (int i = 0; i < MAX_OPENXR_TRACKED_HANDS; i++) {
110 // we'll do this later
111 hand_trackers[i].is_initialized = false;
112 hand_trackers[i].hand_tracker = XR_NULL_HANDLE;
113
114 hand_trackers[i].aimState.aimPose = { { 0.0, 0.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0 } };
115 hand_trackers[i].aimState.pinchStrengthIndex = 0.0;
116 hand_trackers[i].aimState.pinchStrengthMiddle = 0.0;
117 hand_trackers[i].aimState.pinchStrengthRing = 0.0;
118 hand_trackers[i].aimState.pinchStrengthLittle = 0.0;
119
120 hand_trackers[i].locations.isActive = false;
121
122 for (int j = 0; j < XR_HAND_JOINT_COUNT_EXT; j++) {
123 hand_trackers[i].joint_locations[j] = { 0, { { 0.0, 0.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0 } }, 0.0 };
124 hand_trackers[i].joint_velocities[j] = { 0, { 0.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0 } };
125 }
126 }
127}
128
129void OpenXRHandTrackingExtension::on_process() {
130 if (!handTrackingSystemProperties.supportsHandTracking) {
131 // not supported...
132 return;
133 }
134
135 // process our hands
136 const XrTime time = OpenXRAPI::get_singleton()->get_next_frame_time(); // This data will be used for the next frame we render
137 if (time == 0) {
138 // we don't have timing info yet, or we're skipping a frame...
139 return;
140 }
141
142 XrResult result;
143
144 for (int i = 0; i < MAX_OPENXR_TRACKED_HANDS; i++) {
145 if (hand_trackers[i].hand_tracker == XR_NULL_HANDLE) {
146 XrHandTrackerCreateInfoEXT createInfo = {
147 XR_TYPE_HAND_TRACKER_CREATE_INFO_EXT, // type
148 nullptr, // next
149 i == 0 ? XR_HAND_LEFT_EXT : XR_HAND_RIGHT_EXT, // hand
150 XR_HAND_JOINT_SET_DEFAULT_EXT, // handJointSet
151 };
152
153 result = xrCreateHandTrackerEXT(OpenXRAPI::get_singleton()->get_session(), &createInfo, &hand_trackers[i].hand_tracker);
154 if (XR_FAILED(result)) {
155 // not successful? then we do nothing.
156 print_line("OpenXR: Failed to obtain hand tracking information [", OpenXRAPI::get_singleton()->get_error_string(result), "]");
157 hand_trackers[i].is_initialized = false;
158 } else {
159 void *next_pointer = nullptr;
160 if (hand_tracking_aim_state_ext) {
161 hand_trackers[i].aimState.type = XR_TYPE_HAND_TRACKING_AIM_STATE_FB;
162 hand_trackers[i].aimState.next = next_pointer;
163 hand_trackers[i].aimState.status = 0;
164 hand_trackers[i].aimState.aimPose = { { 0.0, 0.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0 } };
165 hand_trackers[i].aimState.pinchStrengthIndex = 0.0;
166 hand_trackers[i].aimState.pinchStrengthMiddle = 0.0;
167 hand_trackers[i].aimState.pinchStrengthRing = 0.0;
168 hand_trackers[i].aimState.pinchStrengthLittle = 0.0;
169
170 next_pointer = &hand_trackers[i].aimState;
171 }
172
173 hand_trackers[i].velocities.type = XR_TYPE_HAND_JOINT_VELOCITIES_EXT;
174 hand_trackers[i].velocities.next = next_pointer;
175 hand_trackers[i].velocities.jointCount = XR_HAND_JOINT_COUNT_EXT;
176 hand_trackers[i].velocities.jointVelocities = hand_trackers[i].joint_velocities;
177 next_pointer = &hand_trackers[i].velocities;
178
179 hand_trackers[i].locations.type = XR_TYPE_HAND_JOINT_LOCATIONS_EXT;
180 hand_trackers[i].locations.next = next_pointer;
181 hand_trackers[i].locations.isActive = false;
182 hand_trackers[i].locations.jointCount = XR_HAND_JOINT_COUNT_EXT;
183 hand_trackers[i].locations.jointLocations = hand_trackers[i].joint_locations;
184
185 hand_trackers[i].is_initialized = true;
186 }
187 }
188
189 if (hand_trackers[i].is_initialized) {
190 void *next_pointer = nullptr;
191
192 XrHandJointsMotionRangeInfoEXT motionRangeInfo;
193
194 if (hand_motion_range_ext) {
195 motionRangeInfo.type = XR_TYPE_HAND_JOINTS_MOTION_RANGE_INFO_EXT;
196 motionRangeInfo.next = next_pointer;
197 motionRangeInfo.handJointsMotionRange = hand_trackers[i].motion_range;
198
199 next_pointer = &motionRangeInfo;
200 }
201
202 XrHandJointsLocateInfoEXT locateInfo = {
203 XR_TYPE_HAND_JOINTS_LOCATE_INFO_EXT, // type
204 next_pointer, // next
205 OpenXRAPI::get_singleton()->get_play_space(), // baseSpace
206 time, // time
207 };
208
209 result = xrLocateHandJointsEXT(hand_trackers[i].hand_tracker, &locateInfo, &hand_trackers[i].locations);
210 if (XR_FAILED(result)) {
211 // not successful? then we do nothing.
212 print_line("OpenXR: Failed to get tracking for hand", i, "[", OpenXRAPI::get_singleton()->get_error_string(result), "]");
213 continue;
214 }
215
216 // For some reason an inactive controller isn't coming back as inactive but has coordinates either as NAN or very large
217 const XrPosef &palm = hand_trackers[i].joint_locations[XR_HAND_JOINT_PALM_EXT].pose;
218 if (
219 !hand_trackers[i].locations.isActive || isnan(palm.position.x) || palm.position.x < -1000000.00 || palm.position.x > 1000000.00) {
220 hand_trackers[i].locations.isActive = false; // workaround, make sure its inactive
221 }
222
223 /* TODO change this to managing the controller from openxr_interface
224 if (hand_tracking_aim_state_ext && hand_trackers[i].locations.isActive && check_bit(XR_HAND_TRACKING_AIM_VALID_BIT_FB, hand_trackers[i].aimState.status)) {
225 // Controllers are updated based on the aim state's pose and pinches' strength
226 if (hand_trackers[i].aim_state_godot_controller == -1) {
227 hand_trackers[i].aim_state_godot_controller =
228 arvr_api->godot_arvr_add_controller(
229 const_cast<char *>(hand_controller_names[i]),
230 i + HAND_CONTROLLER_ID_OFFSET,
231 true,
232 true);
233 }
234 }
235 */
236 }
237 }
238}
239
240void OpenXRHandTrackingExtension::on_state_stopping() {
241 // cleanup
242 cleanup_hand_tracking();
243}
244
245void OpenXRHandTrackingExtension::cleanup_hand_tracking() {
246 XRServer *xr_server = XRServer::get_singleton();
247 ERR_FAIL_NULL(xr_server);
248
249 for (int i = 0; i < MAX_OPENXR_TRACKED_HANDS; i++) {
250 if (hand_trackers[i].hand_tracker != XR_NULL_HANDLE) {
251 xrDestroyHandTrackerEXT(hand_trackers[i].hand_tracker);
252
253 hand_trackers[i].is_initialized = false;
254 hand_trackers[i].hand_tracker = XR_NULL_HANDLE;
255 }
256 }
257}
258
259bool OpenXRHandTrackingExtension::get_active() {
260 return handTrackingSystemProperties.supportsHandTracking;
261}
262
263const OpenXRHandTrackingExtension::HandTracker *OpenXRHandTrackingExtension::get_hand_tracker(uint32_t p_hand) const {
264 ERR_FAIL_UNSIGNED_INDEX_V(p_hand, MAX_OPENXR_TRACKED_HANDS, nullptr);
265
266 return &hand_trackers[p_hand];
267}
268
269XrHandJointsMotionRangeEXT OpenXRHandTrackingExtension::get_motion_range(uint32_t p_hand) const {
270 ERR_FAIL_UNSIGNED_INDEX_V(p_hand, MAX_OPENXR_TRACKED_HANDS, XR_HAND_JOINTS_MOTION_RANGE_MAX_ENUM_EXT);
271
272 return hand_trackers[p_hand].motion_range;
273}
274
275void OpenXRHandTrackingExtension::set_motion_range(uint32_t p_hand, XrHandJointsMotionRangeEXT p_motion_range) {
276 ERR_FAIL_UNSIGNED_INDEX(p_hand, MAX_OPENXR_TRACKED_HANDS);
277 hand_trackers[p_hand].motion_range = p_motion_range;
278}
279
280Quaternion OpenXRHandTrackingExtension::get_hand_joint_rotation(uint32_t p_hand, XrHandJointEXT p_joint) const {
281 ERR_FAIL_UNSIGNED_INDEX_V(p_hand, MAX_OPENXR_TRACKED_HANDS, Quaternion());
282 ERR_FAIL_UNSIGNED_INDEX_V(p_joint, XR_HAND_JOINT_COUNT_EXT, Quaternion());
283
284 if (!hand_trackers[p_hand].is_initialized) {
285 return Quaternion();
286 }
287
288 const XrHandJointLocationEXT &location = hand_trackers[p_hand].joint_locations[p_joint];
289 return Quaternion(location.pose.orientation.x, location.pose.orientation.y, location.pose.orientation.z, location.pose.orientation.w);
290}
291
292Vector3 OpenXRHandTrackingExtension::get_hand_joint_position(uint32_t p_hand, XrHandJointEXT p_joint) const {
293 ERR_FAIL_UNSIGNED_INDEX_V(p_hand, MAX_OPENXR_TRACKED_HANDS, Vector3());
294 ERR_FAIL_UNSIGNED_INDEX_V(p_joint, XR_HAND_JOINT_COUNT_EXT, Vector3());
295
296 if (!hand_trackers[p_hand].is_initialized) {
297 return Vector3();
298 }
299
300 const XrHandJointLocationEXT &location = hand_trackers[p_hand].joint_locations[p_joint];
301 return Vector3(location.pose.position.x, location.pose.position.y, location.pose.position.z);
302}
303
304float OpenXRHandTrackingExtension::get_hand_joint_radius(uint32_t p_hand, XrHandJointEXT p_joint) const {
305 ERR_FAIL_UNSIGNED_INDEX_V(p_hand, MAX_OPENXR_TRACKED_HANDS, 0.0);
306 ERR_FAIL_UNSIGNED_INDEX_V(p_joint, XR_HAND_JOINT_COUNT_EXT, 0.0);
307
308 if (!hand_trackers[p_hand].is_initialized) {
309 return 0.0;
310 }
311
312 return hand_trackers[p_hand].joint_locations[p_joint].radius;
313}
314
315Vector3 OpenXRHandTrackingExtension::get_hand_joint_linear_velocity(uint32_t p_hand, XrHandJointEXT p_joint) const {
316 ERR_FAIL_UNSIGNED_INDEX_V(p_hand, MAX_OPENXR_TRACKED_HANDS, Vector3());
317 ERR_FAIL_UNSIGNED_INDEX_V(p_joint, XR_HAND_JOINT_COUNT_EXT, Vector3());
318
319 if (!hand_trackers[p_hand].is_initialized) {
320 return Vector3();
321 }
322
323 const XrHandJointVelocityEXT &velocity = hand_trackers[p_hand].joint_velocities[p_joint];
324 return Vector3(velocity.linearVelocity.x, velocity.linearVelocity.y, velocity.linearVelocity.z);
325}
326
327Vector3 OpenXRHandTrackingExtension::get_hand_joint_angular_velocity(uint32_t p_hand, XrHandJointEXT p_joint) const {
328 ERR_FAIL_UNSIGNED_INDEX_V(p_hand, MAX_OPENXR_TRACKED_HANDS, Vector3());
329 ERR_FAIL_UNSIGNED_INDEX_V(p_joint, XR_HAND_JOINT_COUNT_EXT, Vector3());
330
331 if (!hand_trackers[p_hand].is_initialized) {
332 return Vector3();
333 }
334
335 const XrHandJointVelocityEXT &velocity = hand_trackers[p_hand].joint_velocities[p_joint];
336 return Vector3(velocity.angularVelocity.x, velocity.angularVelocity.y, velocity.angularVelocity.z);
337}
338