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