The open source OpenXR runtime
matrixfurry.com
1// Copyright 2021, Collabora, Ltd.
2// Copyright 2024-2025, NVIDIA CORPORATION.
3// SPDX-License-Identifier: BSL-1.0
4/*!
5 * @file
6 * @brief Driver to emulate controllers from hand-tracking input
7 * @author Moshi Turner <moshiturner@protonmail.com>
8 * @author Nick Klingensmith <programmerpichu@gmail.com>
9 *
10 * @ingroup drv_cemu
11 */
12
13#include "xrt/xrt_defines.h"
14#include "xrt/xrt_device.h"
15
16#include "os/os_time.h"
17
18#include "math/m_api.h"
19#include "math/m_space.h"
20#include "math/m_vec3.h"
21
22#include "util/u_var.h"
23#include "util/u_time.h"
24#include "util/u_misc.h"
25#include "util/u_debug.h"
26#include "util/u_device.h"
27#include "util/u_distortion_mesh.h"
28#include "util/u_config_json.h"
29
30#include "ht_ctrl_emu_interface.h"
31
32#include <assert.h>
33#include <stdio.h>
34
35
36static const float cm2m = 0.01f;
37
38DEBUG_GET_ONCE_LOG_OPTION(cemu_log, "CEMU_LOG", U_LOGGING_TRACE)
39
40#define CEMU_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->sys->log_level, __VA_ARGS__)
41#define CEMU_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->sys->log_level, __VA_ARGS__)
42#define CEMU_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->sys->log_level, __VA_ARGS__)
43#define CEMU_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->sys->log_level, __VA_ARGS__)
44#define CEMU_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->sys->log_level, __VA_ARGS__)
45
46enum cemu_input_index
47{
48 CEMU_INDEX_HAND_TRACKING,
49 CEMU_INDEX_PINCH_BOOL,
50 CEMU_INDEX_PINCH_FLOAT,
51 CEMU_INDEX_GRIP,
52 CEMU_INDEX_AIM,
53 CEMU_NUM_INPUTS,
54};
55
56static enum xrt_space_relation_flags valid_flags = (enum xrt_space_relation_flags)(
57 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
58 XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
59
60struct cemu_system
61{
62 // We don't own the head - never free this
63 struct xrt_device *in_head;
64 // We "own" the hand, and it gets replaced by the out_hands. So once they are both freed we need to free the
65 // original hand tracker
66 struct xrt_device *in_hand;
67
68 struct cemu_device *out_hand[2];
69
70 float grip_offset_from_palm;
71
72 float waggle, curl, twist;
73
74 enum u_logging_level log_level;
75};
76
77struct cemu_device
78{
79 struct xrt_device base;
80 struct cemu_system *sys;
81
82 int hand_index;
83 enum xrt_input_name ht_input_name;
84
85 struct xrt_tracking_origin tracking_origin;
86};
87
88xrt_quat
89wct_to_quat(float waggle, float curl, float twist)
90{
91 xrt_vec3 waggle_axis = {0, 1, 0};
92 xrt_quat just_waggle;
93 math_quat_from_angle_vector(waggle, &waggle_axis, &just_waggle);
94
95 xrt_vec3 curl_axis = {1, 0, 0};
96 xrt_quat just_curl;
97 math_quat_from_angle_vector(curl, &curl_axis, &just_curl);
98
99 xrt_vec3 twist_axis = {0, 0, 1};
100 xrt_quat just_twist;
101 math_quat_from_angle_vector(twist, &twist_axis, &just_twist);
102
103 xrt_quat out = just_waggle; // Unnecessary but much easier to look at.
104
105 math_quat_rotate(&out, &just_curl, &out);
106 math_quat_rotate(&out, &just_twist, &out);
107 return out;
108}
109
110static inline bool
111find_best_ht_input_names(const struct xrt_device *ht_device, enum xrt_input_name ht_input_names[2])
112{
113 assert(ht_device && ht_device->inputs);
114
115 if (!ht_device->supported.hand_tracking)
116 return false;
117
118 constexpr const enum xrt_input_name XRT_NULL_INPUT_NAME = (enum xrt_input_name)0;
119
120 ht_input_names[0] = ht_input_names[1] = XRT_NULL_INPUT_NAME;
121
122 for (uint32_t i = 0; i < ht_device->input_count; ++i) {
123 if (ht_device->inputs[i].name == XRT_INPUT_HT_UNOBSTRUCTED_LEFT) {
124 ht_input_names[0] = XRT_INPUT_HT_UNOBSTRUCTED_LEFT;
125 }
126
127 if (ht_device->inputs[i].name == XRT_INPUT_HT_UNOBSTRUCTED_RIGHT) {
128 ht_input_names[1] = XRT_INPUT_HT_UNOBSTRUCTED_RIGHT;
129 }
130 }
131
132 for (uint32_t i = 0; i < ht_device->input_count; ++i) {
133 if (ht_input_names[0] == XRT_NULL_INPUT_NAME &&
134 ht_device->inputs[i].name == XRT_INPUT_HT_CONFORMING_LEFT) {
135 ht_input_names[0] = XRT_INPUT_HT_CONFORMING_LEFT;
136 }
137
138 if (ht_input_names[1] == XRT_NULL_INPUT_NAME &&
139 ht_device->inputs[i].name == XRT_INPUT_HT_CONFORMING_RIGHT) {
140 ht_input_names[1] = XRT_INPUT_HT_CONFORMING_RIGHT;
141 }
142 }
143
144 return ht_input_names[0] != XRT_NULL_INPUT_NAME && //
145 ht_input_names[1] != XRT_NULL_INPUT_NAME;
146}
147
148static inline struct cemu_device *
149cemu_device(struct xrt_device *xdev)
150{
151 return (struct cemu_device *)xdev;
152}
153
154
155static void
156cemu_device_destroy(struct xrt_device *xdev)
157{
158 struct cemu_device *dev = cemu_device(xdev);
159 struct cemu_system *system = dev->sys;
160
161 // Remove the variable tracking.
162 u_device_free(&system->out_hand[dev->hand_index]->base);
163
164 system->out_hand[dev->hand_index] = NULL;
165
166 if ((system->out_hand[0] == NULL) && (system->out_hand[1] == NULL)) {
167 xrt_device_destroy(&system->in_hand);
168 u_var_remove_root(system);
169 free(system);
170 }
171}
172
173static xrt_result_t
174cemu_device_get_hand_tracking(struct xrt_device *xdev,
175 enum xrt_input_name name,
176 int64_t requested_timestamp_ns,
177 struct xrt_hand_joint_set *out_value,
178 int64_t *out_timestamp_ns)
179{
180 // Shadows normal hand tracking - does nothing differently
181
182 struct cemu_device *dev = cemu_device(xdev);
183 struct cemu_system *system = dev->sys;
184
185 if (name != dev->ht_input_name) {
186 U_LOG_XDEV_UNSUPPORTED_INPUT(&dev->base, system->log_level, name);
187 return XRT_ERROR_INPUT_UNSUPPORTED;
188 }
189
190 return xrt_device_get_hand_tracking(system->in_hand, dev->ht_input_name, requested_timestamp_ns, out_value,
191 out_timestamp_ns);
192}
193
194static xrt_vec3
195joint_position_global(xrt_hand_joint_set *joint_set, xrt_hand_joint joint)
196{
197 struct xrt_space_relation out_relation;
198 struct xrt_relation_chain xrc = {};
199 m_relation_chain_push_relation(&xrc, &joint_set->values.hand_joint_set_default[joint].relation);
200 m_relation_chain_push_relation(&xrc, &joint_set->hand_pose);
201 m_relation_chain_resolve(&xrc, &out_relation);
202 return out_relation.pose.position;
203}
204
205static xrt_pose
206joint_pose_global(xrt_hand_joint_set *joint_set, xrt_hand_joint joint)
207{
208 struct xrt_space_relation out_relation;
209 struct xrt_relation_chain xrc = {};
210 m_relation_chain_push_relation(&xrc, &joint_set->values.hand_joint_set_default[joint].relation);
211 m_relation_chain_push_relation(&xrc, &joint_set->hand_pose);
212 m_relation_chain_resolve(&xrc, &out_relation);
213 return out_relation.pose;
214}
215
216static xrt_result_t
217do_grip_pose(struct xrt_hand_joint_set *joint_set,
218 struct xrt_space_relation *out_relation,
219 float grip_offset_from_palm,
220 bool is_right)
221{
222
223 xrt_pose offset_from_palm;
224 math_pose_identity(&offset_from_palm);
225 offset_from_palm.position.y = -grip_offset_from_palm;
226 xrt_pose palm = joint_pose_global(joint_set, XRT_HAND_JOINT_PALM);
227
228 // Position.
229 struct xrt_relation_chain xrc = {};
230 m_relation_chain_push_pose(&xrc, &offset_from_palm);
231 m_relation_chain_push_pose(&xrc, &palm);
232 m_relation_chain_resolve(&xrc, out_relation);
233
234
235 // Orientation.
236 xrt_vec3 indx_position = joint_position_global(joint_set, XRT_HAND_JOINT_INDEX_PROXIMAL);
237 xrt_vec3 ring_position = joint_position_global(joint_set, XRT_HAND_JOINT_RING_PROXIMAL);
238 struct xrt_vec3 plus_z = ring_position - indx_position;
239 struct xrt_vec3 plus_x;
240 struct xrt_vec3 to_rotate = {0.0f, is_right ? 1.0f : -1.0f, 0.0f};
241
242 math_quat_rotate_vec3(&palm.orientation, &to_rotate, &plus_x);
243
244 plus_x = m_vec3_orthonormalize(plus_z, plus_x);
245
246 math_vec3_normalize(&plus_x);
247 math_vec3_normalize(&plus_z);
248
249 math_quat_from_plus_x_z(&plus_x, &plus_z, &out_relation->pose.orientation);
250
251 out_relation->relation_flags = valid_flags;
252
253 return XRT_SUCCESS;
254}
255
256
257
258static xrt_result_t
259get_other_two(struct cemu_device *dev,
260 int64_t head_timestamp_ns,
261 int64_t hand_timestamp_ns,
262 xrt_pose *out_head,
263 xrt_hand_joint_set *out_secondary)
264{
265 struct cemu_system *sys = dev->sys;
266 struct xrt_space_relation head_rel;
267 xrt_result_t xret =
268 xrt_device_get_tracked_pose(sys->in_head, XRT_INPUT_GENERIC_HEAD_POSE, head_timestamp_ns, &head_rel);
269 U_LOG_CHK_AND_RET(sys->log_level, xret, "xrt_device_get_tracked_pose");
270
271 *out_head = head_rel.pose;
272 int other;
273 if (dev->hand_index == 0) {
274 other = 1;
275 } else {
276 other = 0;
277 }
278
279 int64_t noop;
280 return xrt_device_get_hand_tracking(sys->in_hand, sys->out_hand[other]->ht_input_name, hand_timestamp_ns,
281 out_secondary, &noop);
282}
283
284// Mostly stolen from
285// https://github.com/maluoi/StereoKit/blob/048b689f71d080a67fde29838c0362a49b88b3d6/StereoKitC/systems/hand/hand_oxr_articulated.cpp#L149
286static xrt_result_t
287do_aim_pose(struct cemu_device *dev,
288 struct xrt_hand_joint_set *joint_set_primary,
289 int64_t head_timestamp_ns,
290 int64_t hand_timestamp_ns,
291 struct xrt_space_relation *out_relation)
292{
293 struct cemu_system *sys = dev->sys;
294
295 struct xrt_vec3 vec3_up = {0, 1, 0};
296 struct xrt_pose head;
297 struct xrt_hand_joint_set joint_set_secondary;
298#if 0
299 // "Jakob way"
300 xrt_result_t xret = get_other_two(dev, hand_timestamp_ns, hand_timestamp_ns, &head, &joint_set_secondary);
301#else
302 // "Moshi way"
303 xrt_result_t xret = get_other_two(dev, head_timestamp_ns, hand_timestamp_ns, &head, &joint_set_secondary);
304#endif
305 U_LOG_CHK_AND_RET(sys->log_level, xret, "get_other_two");
306
307 // Average shoulder width for women:37cm, men:41cm, center of shoulder
308 // joint is around 4cm inwards
309 const float avg_shoulder_width = ((39.0f / 2.0f) - 4.0f) * cm2m;
310 const float head_length = 10 * cm2m;
311 const float neck_length = 7 * cm2m;
312
313 // Chest center is down to the base of the head, and then down the neck.
314 xrt_vec3 down_the_base_of_head;
315 xrt_vec3 base_head_direction = {0, -head_length, 0};
316
317 math_quat_rotate_vec3(&head.orientation, &base_head_direction, &down_the_base_of_head);
318
319 xrt_vec3 chest_center = head.position + down_the_base_of_head + xrt_vec3{0, -neck_length, 0};
320
321 xrt_vec3 face_fwd;
322 xrt_vec3 forwards = {0, 0, -1};
323
324 math_quat_rotate_vec3(&head.orientation, &forwards, &face_fwd);
325
326 face_fwd = m_vec3_mul_scalar(m_vec3_normalize(face_fwd), 2);
327 face_fwd += m_vec3_mul_scalar(
328 m_vec3_normalize(joint_position_global(joint_set_primary, XRT_HAND_JOINT_WRIST) - chest_center), 1);
329 if (joint_set_secondary.is_active) {
330 face_fwd += m_vec3_mul_scalar(
331 m_vec3_normalize(joint_position_global(&joint_set_secondary, XRT_HAND_JOINT_WRIST) - chest_center),
332 1);
333 }
334 face_fwd.y = 0;
335 m_vec3_normalize(face_fwd);
336
337 xrt_vec3 face_right;
338 math_vec3_cross(&face_fwd, &vec3_up, &face_right);
339 math_vec3_normalize(&face_right);
340 face_right *= avg_shoulder_width;
341
342 xrt_vec3 shoulder = chest_center + face_right * (dev->hand_index == 1 ? 1.0f : -1.0f);
343
344 xrt_vec3 ray_joint = joint_position_global(joint_set_primary, XRT_HAND_JOINT_INDEX_PROXIMAL);
345
346 struct xrt_vec3 ray_direction = shoulder - ray_joint;
347
348 struct xrt_vec3 up = {0, 1, 0};
349
350 struct xrt_vec3 out_x_vector;
351
352 // math_vec3_normalize(&tip_to_palm);
353 math_vec3_normalize(&ray_direction);
354
355 math_vec3_cross(&up, &ray_direction, &out_x_vector);
356
357 out_relation->pose.position = ray_joint;
358
359 math_quat_from_plus_x_z(&out_x_vector, &ray_direction, &out_relation->pose.orientation);
360
361 out_relation->relation_flags = valid_flags;
362
363 return xret;
364}
365
366// Pose for controller emulation
367static xrt_result_t
368cemu_device_get_tracked_pose(struct xrt_device *xdev,
369 enum xrt_input_name name,
370 int64_t at_timestamp_ns,
371 struct xrt_space_relation *out_relation)
372{
373 struct cemu_device *dev = cemu_device(xdev);
374 struct cemu_system *sys = dev->sys;
375
376 if (name != XRT_INPUT_HAND_CTRL_EMU_AIM_POSE && name != XRT_INPUT_HAND_CTRL_EMU_GRIP_POSE) {
377 U_LOG_XDEV_UNSUPPORTED_INPUT(&dev->base, dev->sys->log_level, name);
378 return XRT_ERROR_INPUT_UNSUPPORTED;
379 }
380 static int64_t hand_timestamp_ns;
381
382 struct xrt_hand_joint_set joint_set;
383 xrt_result_t xret = xrt_device_get_hand_tracking(sys->in_hand, dev->ht_input_name, at_timestamp_ns, &joint_set,
384 &hand_timestamp_ns);
385 U_LOG_CHK_AND_RET(sys->log_level, xret, "xrt_device_get_hand_tracking");
386
387 if (joint_set.is_active == false) {
388 out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE;
389 return XRT_SUCCESS;
390 }
391
392 xret = XRT_SUCCESS;
393 switch (name) {
394 case XRT_INPUT_HAND_CTRL_EMU_GRIP_POSE: {
395 xret = do_grip_pose(&joint_set, out_relation, sys->grip_offset_from_palm, dev->hand_index);
396 break;
397 }
398 case XRT_INPUT_HAND_CTRL_EMU_AIM_POSE: {
399 // Assume that now we're doing everything in the timestamp from the hand-tracker, so use
400 // hand_timestamp_ns. This will cause the controller to lag behind but otherwise be correct
401 xret = do_aim_pose(dev, &joint_set, at_timestamp_ns, hand_timestamp_ns, out_relation);
402 break;
403 }
404 default: assert(false);
405 }
406
407 return xret;
408}
409
410//! @todo This is flickery; investigate once we get better hand tracking
411static void
412decide(xrt_vec3 one, xrt_vec3 two, bool *out)
413{
414 float dist = m_vec3_len_sqrd(one - two);
415 // These used to be 0.02f and 0.04f, but I bumped them way up to compensate for bad tracking. Once our tracking
416 // is better, bump these back down.
417 float activation_dist = 0.02f;
418 float deactivation_dist = 0.04f;
419 const float pinch_activation_dist =
420 (*out ? deactivation_dist * deactivation_dist : activation_dist * activation_dist);
421
422 *out = (dist < pinch_activation_dist);
423}
424
425static xrt_result_t
426cemu_device_update_inputs(struct xrt_device *xdev)
427{
428 struct cemu_device *dev = cemu_device(xdev);
429 struct cemu_system *sys = dev->sys;
430
431 struct xrt_hand_joint_set joint_set;
432 int64_t noop;
433
434 xrt_result_t xret = xrt_device_get_hand_tracking(dev->sys->in_hand, dev->ht_input_name, os_monotonic_get_ns(),
435 &joint_set, &noop);
436 U_LOG_CHK_AND_RET(sys->log_level, xret, "xrt_device_get_hand_tracking");
437
438 if (!joint_set.is_active) {
439 xdev->inputs[CEMU_INDEX_PINCH_BOOL].value.boolean = false;
440 xdev->inputs[CEMU_INDEX_PINCH_FLOAT].value.vec1.x = 0.f;
441 return XRT_SUCCESS;
442 }
443
444 decide(joint_set.values.hand_joint_set_default[XRT_HAND_JOINT_INDEX_TIP].relation.pose.position,
445 joint_set.values.hand_joint_set_default[XRT_HAND_JOINT_THUMB_TIP].relation.pose.position,
446 &xdev->inputs[CEMU_INDEX_PINCH_BOOL].value.boolean);
447
448 // For now, all other inputs are off - detecting any gestures more complicated than pinch is too unreliable for
449 // now.
450 if (xdev->inputs[CEMU_INDEX_PINCH_BOOL].value.boolean) {
451 xdev->inputs[CEMU_INDEX_PINCH_FLOAT].value.vec1.x = 1.0f;
452 } else {
453 xdev->inputs[CEMU_INDEX_PINCH_FLOAT].value.vec1.x = 0.0f;
454 }
455
456 return XRT_SUCCESS;
457}
458
459static struct xrt_binding_input_pair simple_inputs[3] = {
460 {XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_HAND_CTRL_EMU_PINCH_BOOL},
461 {XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_HAND_CTRL_EMU_GRIP_POSE},
462 {XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_HAND_CTRL_EMU_AIM_POSE},
463};
464
465static struct xrt_binding_profile binding_profiles[1] = {
466 {
467 .name = XRT_DEVICE_SIMPLE_CONTROLLER,
468 .inputs = simple_inputs,
469 .input_count = ARRAY_SIZE(simple_inputs),
470 .outputs = nullptr,
471 .output_count = 0,
472 },
473};
474
475extern "C" int
476cemu_devices_create(struct xrt_device *head, struct xrt_device *hands, struct xrt_device **out_xdevs)
477{
478 enum xrt_input_name ht_input_names[2];
479 if (!find_best_ht_input_names(hands, ht_input_names)) {
480 U_LOG_E("\"hands\" parameter is not a hand-tracking device or does have expected inputs.");
481 return 0;
482 }
483
484 enum u_device_alloc_flags flags = U_DEVICE_ALLOC_NO_FLAGS;
485
486 struct cemu_device *cemud[2];
487
488 struct cemu_system *system = U_TYPED_CALLOC(struct cemu_system);
489 system->in_hand = hands;
490 system->in_head = head;
491
492 system->log_level = debug_get_log_option_cemu_log();
493
494 system->grip_offset_from_palm = 0.03f; // 3 centimeters
495
496 for (int i = 0; i < 2; i++) {
497 cemud[i] = U_DEVICE_ALLOCATE(struct cemu_device, flags, CEMU_NUM_INPUTS, 0);
498
499 cemud[i]->sys = system;
500
501 cemud[i]->base.tracking_origin = hands->tracking_origin;
502
503 cemud[i]->base.name = XRT_DEVICE_HAND_CTRL_EMU;
504 cemud[i]->base.supported.hand_tracking = true;
505 cemud[i]->base.supported.orientation_tracking = true;
506 cemud[i]->base.supported.position_tracking = true;
507 cemud[i]->base.binding_profiles = binding_profiles;
508 cemud[i]->base.binding_profile_count = ARRAY_SIZE(binding_profiles);
509
510 cemud[i]->base.inputs[CEMU_INDEX_HAND_TRACKING].name = ht_input_names[i];
511 cemud[i]->base.inputs[CEMU_INDEX_PINCH_BOOL].name = XRT_INPUT_HAND_CTRL_EMU_PINCH_BOOL;
512 cemud[i]->base.inputs[CEMU_INDEX_PINCH_FLOAT].name = XRT_INPUT_HAND_CTRL_EMU_PINCH_VALUE;
513 cemud[i]->base.inputs[CEMU_INDEX_GRIP].name = XRT_INPUT_HAND_CTRL_EMU_GRIP_POSE;
514 cemud[i]->base.inputs[CEMU_INDEX_AIM].name = XRT_INPUT_HAND_CTRL_EMU_AIM_POSE;
515
516 cemud[i]->base.update_inputs = cemu_device_update_inputs;
517 cemud[i]->base.get_tracked_pose = cemu_device_get_tracked_pose;
518 cemud[i]->base.set_output = u_device_ni_set_output;
519 cemud[i]->base.get_hand_tracking = cemu_device_get_hand_tracking;
520 cemud[i]->base.destroy = cemu_device_destroy;
521
522 cemud[i]->base.device_type =
523 i ? XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER : XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER;
524
525 int n =
526 snprintf(cemud[i]->base.str, XRT_DEVICE_NAME_LEN, "%s %s Hand", i ? "Right" : "Left", hands->str);
527 if (n > XRT_DEVICE_NAME_LEN) {
528 CEMU_DEBUG(cemud[i], "name truncated: %s", cemud[i]->base.str);
529 }
530
531 n = snprintf(cemud[i]->base.serial, XRT_DEVICE_NAME_LEN, "%s (%d)", hands->str, i);
532 if (n > XRT_DEVICE_NAME_LEN) {
533 CEMU_WARN(cemud[i], "serial truncated: %s", cemud[i]->base.str);
534 }
535
536 cemud[i]->ht_input_name = ht_input_names[i];
537 cemud[i]->hand_index = i;
538 system->out_hand[i] = cemud[i];
539
540 out_xdevs[i] = &cemud[i]->base;
541 }
542
543 u_var_add_root(system, "Controller emulation!", true);
544 u_var_add_f32(system, &system->grip_offset_from_palm, "Grip pose offset");
545
546 return 2;
547
548 // We actually don't need these - no failure condition yet. Uncomment whenever you need 'em
549 // cleanup:
550 // cemu_device_destroy(&cemud[0]->base);
551 // cemu_device_destroy(&cemud[1]->base);
552 // return 0;
553}