The open source OpenXR runtime
0
fork

Configure Feed

Select the types of activity you want to include in your feed.

d/ht_ctrl_emu: Create the driver

+487
+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
··· 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
··· 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