matrixfurry.com
The open source OpenXR runtime
1// Copyright 2023, Shawn Wallace
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief SteamVR driver device implementation.
6 * @author Shawn Wallace <yungwallace@live.com>
7 * @author Beyley Cardellio <ep1cm1n10n123@gmail.com>
8 * @ingroup drv_steamvr_lh
9 */
10
11#include "math/m_api.h"
12#include "math/m_predict.h"
13#include "math/m_relation_history.h"
14#include "math/m_space.h"
15
16#include "interfaces/context.hpp"
17
18#include "util/u_debug.h"
19#include "util/u_device.h"
20#include "util/u_hand_tracking.h"
21#include "util/u_logging.h"
22#include "util/u_json.hpp"
23
24#include "util/u_time.h"
25#include "xrt/xrt_defines.h"
26#include "xrt/xrt_device.h"
27#include "xrt/xrt_prober.h"
28
29#include "vive/vive_poses.h"
30
31#include "openvr_driver.h"
32
33#include "device.hpp"
34
35#include <cmath>
36#include <functional>
37#include <cstring>
38#include <numbers>
39#include <openvr_driver.h>
40#include <thread>
41#include <algorithm>
42#include <map>
43
44#define DEV_ERR(...) U_LOG_IFL_E(ctx->log_level, __VA_ARGS__)
45#define DEV_WARN(...) U_LOG_IFL_W(ctx->log_level, __VA_ARGS__)
46#define DEV_INFO(...) U_LOG_IFL_I(ctx->log_level, __VA_ARGS__)
47#define DEV_DEBUG(...) U_LOG_IFL_D(ctx->log_level, __VA_ARGS__)
48
49DEBUG_GET_ONCE_BOOL_OPTION(lh_emulate_hand, "LH_EMULATE_HAND", true)
50
51// Each device will have its own input class.
52struct InputClass
53{
54 xrt_device_name name;
55 const std::vector<xrt_input_name> poses;
56 const std::unordered_map<std::string_view, xrt_input_name> non_poses;
57};
58
59namespace {
60using namespace std::string_view_literals;
61// From https://github.com/ValveSoftware/openvr/blob/master/docs/Driver_API_Documentation.md#bone-structure
62enum HandSkeletonBone : int32_t
63{
64 eBone_Root = 0,
65 eBone_Wrist,
66 eBone_Thumb0,
67 eBone_Thumb1,
68 eBone_Thumb2,
69 eBone_Thumb3,
70 eBone_IndexFinger0,
71 eBone_IndexFinger1,
72 eBone_IndexFinger2,
73 eBone_IndexFinger3,
74 eBone_IndexFinger4,
75 eBone_MiddleFinger0,
76 eBone_MiddleFinger1,
77 eBone_MiddleFinger2,
78 eBone_MiddleFinger3,
79 eBone_MiddleFinger4,
80 eBone_RingFinger0,
81 eBone_RingFinger1,
82 eBone_RingFinger2,
83 eBone_RingFinger3,
84 eBone_RingFinger4,
85 eBone_PinkyFinger0,
86 eBone_PinkyFinger1,
87 eBone_PinkyFinger2,
88 eBone_PinkyFinger3,
89 eBone_PinkyFinger4,
90 eBone_Aux_Thumb,
91 eBone_Aux_IndexFinger,
92 eBone_Aux_MiddleFinger,
93 eBone_Aux_RingFinger,
94 eBone_Aux_PinkyFinger,
95 eBone_Count
96};
97
98// Adding support for a new controller is a simple as adding it here.
99// The key for the map needs to be the name of input profile as indicated by the lighthouse driver.
100const std::unordered_map<std::string_view, InputClass> controller_classes{
101 {
102 "vive_controller",
103 InputClass{
104 XRT_DEVICE_VIVE_WAND,
105 {
106 XRT_INPUT_VIVE_GRIP_POSE,
107 XRT_INPUT_VIVE_AIM_POSE,
108 XRT_INPUT_GENERIC_PALM_POSE,
109 },
110 {
111 {"/input/application_menu/click", XRT_INPUT_VIVE_MENU_CLICK},
112 {"/input/trackpad/click", XRT_INPUT_VIVE_TRACKPAD_CLICK},
113 {"/input/trackpad/touch", XRT_INPUT_VIVE_TRACKPAD_TOUCH},
114 {"/input/system/click", XRT_INPUT_VIVE_SYSTEM_CLICK},
115 {"/input/trigger/click", XRT_INPUT_VIVE_TRIGGER_CLICK},
116 {"/input/trigger/value", XRT_INPUT_VIVE_TRIGGER_VALUE},
117 {"/input/grip/click", XRT_INPUT_VIVE_SQUEEZE_CLICK},
118 {"/input/trackpad", XRT_INPUT_VIVE_TRACKPAD},
119 },
120 },
121 },
122 {
123 "index_controller",
124 InputClass{
125 XRT_DEVICE_INDEX_CONTROLLER,
126 {
127 XRT_INPUT_INDEX_GRIP_POSE,
128 XRT_INPUT_INDEX_AIM_POSE,
129 XRT_INPUT_GENERIC_PALM_POSE,
130 },
131 {
132 {"/input/system/click", XRT_INPUT_INDEX_SYSTEM_CLICK},
133 {"/input/system/touch", XRT_INPUT_INDEX_SYSTEM_TOUCH},
134 {"/input/a/click", XRT_INPUT_INDEX_A_CLICK},
135 {"/input/a/touch", XRT_INPUT_INDEX_A_TOUCH},
136 {"/input/b/click", XRT_INPUT_INDEX_B_CLICK},
137 {"/input/b/touch", XRT_INPUT_INDEX_B_TOUCH},
138 {"/input/trigger/click", XRT_INPUT_INDEX_TRIGGER_CLICK},
139 {"/input/trigger/touch", XRT_INPUT_INDEX_TRIGGER_TOUCH},
140 {"/input/trigger/value", XRT_INPUT_INDEX_TRIGGER_VALUE},
141 {"/input/grip/force", XRT_INPUT_INDEX_SQUEEZE_FORCE},
142 {"/input/grip/value", XRT_INPUT_INDEX_SQUEEZE_VALUE},
143 {"/input/thumbstick/click", XRT_INPUT_INDEX_THUMBSTICK_CLICK},
144 {"/input/thumbstick/touch", XRT_INPUT_INDEX_THUMBSTICK_TOUCH},
145 {"/input/thumbstick", XRT_INPUT_INDEX_THUMBSTICK},
146 {"/input/trackpad/force", XRT_INPUT_INDEX_TRACKPAD_FORCE},
147 {"/input/trackpad/touch", XRT_INPUT_INDEX_TRACKPAD_TOUCH},
148 {"/input/trackpad", XRT_INPUT_INDEX_TRACKPAD},
149 },
150 },
151 },
152 {
153 "vive_tracker",
154 InputClass{
155 XRT_DEVICE_VIVE_TRACKER,
156 {
157 XRT_INPUT_GENERIC_TRACKER_POSE,
158 },
159 {
160 {"/input/power/click", XRT_INPUT_VIVE_TRACKER_SYSTEM_CLICK},
161 {"/input/grip/click", XRT_INPUT_VIVE_TRACKER_SQUEEZE_CLICK},
162 {"/input/application_menu/click", XRT_INPUT_VIVE_TRACKER_MENU_CLICK},
163 {"/input/trigger/click", XRT_INPUT_VIVE_TRACKER_TRIGGER_CLICK},
164 {"/input/thumb/click", XRT_INPUT_VIVE_TRACKER_TRACKPAD_CLICK},
165 },
166 },
167 },
168 {
169 "tundra_tracker",
170 InputClass{
171 XRT_DEVICE_VIVE_TRACKER,
172 {
173 XRT_INPUT_GENERIC_TRACKER_POSE,
174 },
175 {
176 {"/input/power/click", XRT_INPUT_VIVE_TRACKER_SYSTEM_CLICK},
177 {"/input/grip/click", XRT_INPUT_VIVE_TRACKER_SQUEEZE_CLICK},
178 {"/input/application_menu/click", XRT_INPUT_VIVE_TRACKER_MENU_CLICK},
179 {"/input/trigger/click", XRT_INPUT_VIVE_TRACKER_TRIGGER_CLICK},
180 {"/input/thumb/click", XRT_INPUT_VIVE_TRACKER_TRACKPAD_CLICK},
181 },
182 },
183 },
184};
185int64_t
186chrono_timestamp_ns()
187{
188 auto now = std::chrono::steady_clock::now().time_since_epoch();
189 int64_t ts = std::chrono::duration_cast<std::chrono::nanoseconds>(now).count();
190 return ts;
191}
192
193// Template for calling a member function of Device from a free function
194template <typename DeviceType, auto Func, typename Ret, typename... Args>
195inline Ret
196device_bouncer(struct xrt_device *xdev, Args... args)
197{
198 auto *dev = static_cast<DeviceType *>(xdev);
199 return std::invoke(Func, dev, args...);
200}
201
202// Setting used for brightness in the steamvr section. This isn't defined by the openvr header.
203static const char *analog_gain_settings_key = "analogGain";
204
205/**
206 * Map a 0-1 (or > 1) brightness value into the analogGain value stored in SteamVR settings.
207 */
208float
209brightness_to_analog_gain(float brightness)
210{
211 // Lookup table from brightness to analog gain value
212 // Courtesy of OyasumiVR, MIT license, Copyright (c) 2022 Raphiiko
213 // https://github.com/Raphiiko/OyasumiVR/blob/c9e7fbcc2ea6caa07a8233a75218598087043171/src-ui/app/services/brightness-control/hardware-brightness-drivers/valve-index-hardware-brightness-control-driver.ts#L92
214 // TODO: We should support having a lookup table per headset model. If not present, fallback to lerp between the
215 // given min and max analog gain. Maybe we can assume 100% brightness = 1.0 analog gain, but we need info from
216 // more headsets.
217 static const auto lookup = std::map<float, float>{{
218 {0.20, 0.03}, {0.23, 0.04}, {0.26, 0.05}, {0.27, 0.055}, {0.28, 0.06}, {0.30, 0.07}, {0.32, 0.08},
219 {0.33, 0.09}, {0.34, 0.095}, {0.35, 0.1}, {0.36, 0.105}, {0.37, 0.11}, {0.37, 0.115}, {0.38, 0.12},
220 {0.39, 0.125}, {0.40, 0.13}, {0.40, 0.135}, {0.41, 0.14}, {0.42, 0.145}, {0.42, 0.15}, {0.43, 0.155},
221 {0.43, 0.16}, {0.44, 0.165}, {0.45, 0.17}, {0.45, 0.175}, {0.46, 0.18}, {0.46, 0.185}, {0.47, 0.19},
222 {0.48, 0.195}, {0.48, 0.2}, {0.49, 0.21}, {0.53, 0.25}, {0.58, 0.3}, {0.59, 0.315}, {0.60, 0.32},
223 {0.60, 0.33}, {0.61, 0.34}, {0.62, 0.35}, {0.66, 0.4}, {0.69, 0.445}, {0.70, 0.45}, {0.70, 0.46},
224 {0.71, 0.465}, {0.71, 0.47}, {0.71, 0.475}, {0.72, 0.48}, {0.72, 0.49}, {0.73, 0.5}, {0.79, 0.6},
225 {0.85, 0.7}, {0.90, 0.8}, {0.95, 0.9}, {1.00, 1}, {1.50, 1.50},
226 }};
227
228 if (const auto upper_it = lookup.upper_bound(brightness); upper_it == lookup.end()) {
229 return lookup.rbegin()->second;
230 } else if (upper_it == lookup.begin()) {
231 return upper_it->second;
232 } else {
233 // Linearly interpolate between the greater and lower points
234 const auto lower_it = std::prev(upper_it);
235 const auto brightness_range = (upper_it->first - lower_it->first);
236 const auto blend_amount = ((brightness - lower_it->first) / brightness_range);
237 return std::lerp(lower_it->second, upper_it->second, blend_amount);
238 }
239
240 return brightness;
241}
242xrt_pose
243bone_to_pose(const vr::VRBoneTransform_t &bone)
244{
245 return xrt_pose{xrt_quat{bone.orientation.x, bone.orientation.y, bone.orientation.z, bone.orientation.w},
246 xrt_vec3{bone.position.v[0], bone.position.v[1], bone.position.v[2]}};
247}
248} // namespace
249
250Property::Property(vr::PropertyTypeTag_t tag, void *buffer, uint32_t bufferSize)
251{
252 this->tag = tag;
253 this->buffer.resize(bufferSize);
254 std::memcpy(this->buffer.data(), buffer, bufferSize);
255}
256
257HmdDevice::HmdDevice(const DeviceBuilder &builder) : Device(builder)
258{
259 this->name = XRT_DEVICE_GENERIC_HMD;
260 this->device_type = XRT_DEVICE_TYPE_HMD;
261 this->container_handle = 0;
262
263 inputs_vec = {xrt_input{true, 0, XRT_INPUT_GENERIC_HEAD_POSE, {}}};
264 this->inputs = inputs_vec.data();
265 this->input_count = inputs_vec.size();
266
267#define SETUP_MEMBER_FUNC(name) this->xrt_device::name = &device_bouncer<HmdDevice, &HmdDevice::name, xrt_result_t>
268 SETUP_MEMBER_FUNC(get_view_poses);
269 SETUP_MEMBER_FUNC(compute_distortion);
270 SETUP_MEMBER_FUNC(set_brightness);
271 SETUP_MEMBER_FUNC(get_brightness);
272#undef SETUP_MEMBER_FUNC
273}
274
275ControllerDevice::ControllerDevice(vr::PropertyContainerHandle_t handle, const DeviceBuilder &builder) : Device(builder)
276{
277 this->device_type = XRT_DEVICE_TYPE_UNKNOWN;
278 this->container_handle = handle;
279
280 this->xrt_device::get_hand_tracking =
281 &device_bouncer<ControllerDevice, &ControllerDevice::get_hand_tracking, xrt_result_t>;
282 this->xrt_device::set_output = &device_bouncer<ControllerDevice, &ControllerDevice::set_output, xrt_result_t>;
283
284 this->inputs_map["/skeleton/hand/left"] = &hand_tracking_inputs[XRT_HAND_LEFT];
285 this->inputs_map["/skeleton/hand/right"] = &hand_tracking_inputs[XRT_HAND_RIGHT];
286}
287
288Device::~Device()
289{
290 m_relation_history_destroy(&relation_hist);
291}
292
293Device::Device(const DeviceBuilder &builder) : xrt_device({}), ctx(builder.ctx), driver(builder.driver)
294{
295 m_relation_history_create(&relation_hist);
296 std::strncpy(this->serial, builder.serial, XRT_DEVICE_NAME_LEN - 1);
297 this->serial[XRT_DEVICE_NAME_LEN - 1] = 0;
298 this->tracking_origin = ctx.get();
299 this->supported.orientation_tracking = true;
300 this->supported.position_tracking = true;
301 this->supported.hand_tracking = true;
302 this->supported.force_feedback = false;
303 this->supported.form_factor_check = false;
304 this->supported.battery_status = true;
305 this->supported.brightness_control = true;
306
307 this->xrt_device::update_inputs = &device_bouncer<Device, &Device::update_inputs, xrt_result_t>;
308#define SETUP_MEMBER_FUNC(name) this->xrt_device::name = &device_bouncer<Device, &Device::name>
309 SETUP_MEMBER_FUNC(get_tracked_pose);
310 SETUP_MEMBER_FUNC(get_battery_status);
311#undef SETUP_MEMBER_FUNC
312
313 this->xrt_device::destroy = [](xrt_device *xdev) {
314 auto *dev = static_cast<Device *>(xdev);
315 dev->driver->Deactivate();
316 delete dev;
317 };
318
319 init_chaperone(builder.steam_install);
320}
321
322// NOTE: No operations that would force inputs_vec or finger_inputs_vec to reallocate (such as insertion)
323// should be done after this function is called, otherwise the pointers in inputs_map/finger_inputs_map
324// would be invalidated.
325void
326ControllerDevice::set_input_class(const InputClass *input_class)
327{
328 // this should only be called once
329 assert(inputs_vec.empty());
330 this->input_class = input_class;
331
332 // reserve to ensure our pointers don't get invalidated
333 inputs_vec.reserve(input_class->poses.size() + input_class->non_poses.size() + 1);
334 for (xrt_input_name input : input_class->poses) {
335 inputs_vec.push_back({true, 0, input, {}});
336 }
337 for (const auto &[path, input] : input_class->non_poses) {
338 assert(inputs_vec.capacity() >= inputs_vec.size() + 1);
339 inputs_vec.push_back({true, 0, input, {}});
340 inputs_map.insert({path, &inputs_vec.back()});
341 }
342
343 this->inputs = inputs_vec.data();
344 this->input_count = inputs_vec.size();
345}
346
347xrt_hand
348ControllerDevice::get_xrt_hand()
349{
350 switch (this->device_type) {
351 case XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER: {
352 return xrt_hand::XRT_HAND_LEFT;
353 }
354 case XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER: {
355 return xrt_hand::XRT_HAND_RIGHT;
356 }
357 default: DEV_ERR("Device %s cannot be tracked as a hand!", serial); return xrt_hand::XRT_HAND_LEFT;
358 }
359}
360
361void
362ControllerDevice::set_active_hand(xrt_hand hand)
363{
364 this->skeleton_hand = hand;
365}
366
367namespace {
368xrt_quat
369from_euler_angles(float x, float y, float z)
370{
371 const xrt_vec3 v{x, y, z};
372 xrt_quat out;
373 math_quat_from_euler_angles(&v, &out);
374 return out;
375}
376
377constexpr float pi = std::numbers::pi_v<float>;
378constexpr float frac_pi_2 = pi / 2.0f;
379
380// OpenVR skeletal poses are defined with the palms facing each other, but OpenXR
381// hand tracking is defined with the palms facing down. These per hand rotations
382// are necessary to translate to what OpenXR expects.
383const xrt_quat right_hand_rotate = from_euler_angles(0.0f, frac_pi_2, 0.0f);
384const xrt_quat left_hand_rotate = from_euler_angles(0.0f, frac_pi_2, pi);
385
386xrt_quat
387right_wrist_rotate_init()
388{
389 const xrt_quat rot1 = from_euler_angles(0.0f, 0.0f, frac_pi_2);
390 const xrt_quat rot2 = from_euler_angles(0.0f, pi, 0.0f);
391 xrt_quat ret;
392 math_quat_rotate(&rot1, &rot2, &ret);
393 return ret;
394}
395xrt_quat
396left_wrist_rotate_init()
397{
398 const xrt_quat rot1 = from_euler_angles(pi, 0.0f, 0.0f);
399 const xrt_quat rot2 = from_euler_angles(0.0f, 0.0f, -frac_pi_2);
400 xrt_quat ret;
401 math_quat_rotate(&rot1, &rot2, &ret);
402 return ret;
403}
404
405const xrt_quat right_wrist_rotate = right_wrist_rotate_init();
406const xrt_quat left_wrist_rotate = left_wrist_rotate_init();
407
408xrt_pose
409generate_palm_pose(const xrt_pose &metacarpal_pose, const xrt_pose &proximal_pose)
410{
411 // OpenVR doesn't provide a palm joint, but the OpenXR palm is in the middle of
412 // the metacarpal and proximal bones of the middle finger,
413 // so we'll interpolate between them to generate it.
414 xrt_pose pose;
415 math_pose_interpolate(&metacarpal_pose, &proximal_pose, 0.5, &pose);
416 // Use metacarpal orientation, because the palm shouldn't really rotate
417 pose.orientation = metacarpal_pose.orientation;
418 return pose;
419}
420
421xrt_pose
422palm_offset_index(xrt_hand hand)
423{
424 // Taken from:
425 // https://github.com/ValveSoftware/OpenXR-Canonical-Pose-Tool/blob/5e6f3f6db584d58483058ff3262e7eef02c3acfd/dist/steamvr/cpt_SteamVR-valve_index_controller.xml#L1
426 switch (hand) {
427 case XRT_HAND_LEFT:
428 return xrt_pose{.orientation = xrt_quat{.x = -0.46, .y = -0.02, .z = -0.01, .w = 0.89},
429 .position = xrt_vec3{.x = -0.015, .y = 0.0, .z = 0.001}};
430 case XRT_HAND_RIGHT:
431 return xrt_pose{.orientation = xrt_quat{.x = -0.46, .y = 0.02, .z = 0.01, .w = 0.89},
432 .position = xrt_vec3{.x = 0.015, .y = 0.0, .z = 0.001}};
433 }
434
435 return {};
436}
437
438} // namespace
439
440void
441ControllerDevice::set_skeleton(std::span<const vr::VRBoneTransform_t> bones,
442 xrt_hand hand,
443 bool is_simulated,
444 const char *path)
445{
446 assert(bones.size() == eBone_Count);
447 generate_palm_pose_offset(bones, hand);
448 if (!is_simulated && debug_get_bool_option_lh_emulate_hand()) {
449 assert(inputs_vec.capacity() >= inputs_vec.size() + 1);
450 const xrt_input_name tracker_name =
451 (hand == XRT_HAND_RIGHT) ? XRT_INPUT_HT_CONFORMING_RIGHT : XRT_INPUT_HT_CONFORMING_LEFT;
452 inputs_vec.push_back({true, 0, tracker_name, {}});
453 inputs_map.insert({path, &inputs_vec.back()});
454 this->input_count = inputs_vec.size();
455 has_hand_tracking = true;
456 }
457}
458
459void
460ControllerDevice::generate_palm_pose_offset(std::span<const vr::VRBoneTransform_t> bones, xrt_hand hand)
461{
462 if (this->input_class->name == XRT_DEVICE_INDEX_CONTROLLER) {
463 xrt_pose grip_offset;
464 vive_poses_get_pose_offset(this->input_class->name, this->device_type, XRT_INPUT_INDEX_GRIP_POSE,
465 &grip_offset);
466 auto offset = palm_offset_index(hand);
467 math_pose_transform(&grip_offset, &offset, &offset);
468 palm_offsets[hand] = offset;
469 return;
470 }
471 // The palm pose offset is generated from the OpenVR provided skeleton.
472 // https://github.com/ValveSoftware/openvr/blob/master/docs/Driver_API_Documentation.md#notes-on-the-skeleton
473
474 xrt_pose root = bone_to_pose(bones[eBone_Root]);
475 xrt_pose wrist = bone_to_pose(bones[eBone_Wrist]);
476 xrt_pose metacarpal = bone_to_pose(bones[eBone_MiddleFinger0]);
477 xrt_pose proximal = bone_to_pose(bones[eBone_MiddleFinger1]);
478
479 // The skeleton pose is given with the Root bone as origin.
480 // To convert from this, according to OpenVR docs we transform the wrist
481 // and then counter-transform the metacarpals
482 xrt_pose root_inv;
483 math_pose_invert(&root, &root_inv);
484 math_pose_transform(&root_inv, &wrist, &wrist);
485 math_pose_transform(&root, &metacarpal, &metacarpal);
486 math_pose_transform(&wrist, &metacarpal, &metacarpal);
487 math_pose_transform(&metacarpal, &proximal, &proximal);
488
489 xrt_pose palm_offset = generate_palm_pose(metacarpal, proximal);
490 xrt_quat palm_rotate = from_euler_angles(0.0f, 0.0f, frac_pi_2);
491
492 switch (hand) {
493 case XRT_HAND_LEFT: {
494 math_quat_rotate(&palm_offset.orientation, &left_hand_rotate, &palm_offset.orientation);
495 math_quat_invert(&palm_rotate, &palm_rotate);
496 break;
497 }
498 case XRT_HAND_RIGHT: {
499 math_quat_rotate(&palm_offset.orientation, &right_hand_rotate, &palm_offset.orientation);
500 break;
501 }
502 }
503 math_quat_rotate(&palm_offset.orientation, &palm_rotate, &palm_offset.orientation);
504
505 // For controllers like the Vive Wands which can be in any hand, it will store both the left hand
506 // and the right hand skeletons, so we need to store both.
507 palm_offsets[hand] = palm_offset;
508}
509
510void
511ControllerDevice::update_skeleton_transforms(std::span<const vr::VRBoneTransform_t> bones)
512{
513 if (!has_hand_tracking) {
514 return;
515 }
516
517 assert(bones.size() == eBone_Count);
518
519 xrt_hand_joint_set joint_set;
520 int64_t timestamp;
521 if (!m_relation_history_get_latest(relation_hist, ×tamp, &joint_set.hand_pose)) {
522 return;
523 }
524 joint_set.is_active = true;
525 auto &joints = joint_set.values.hand_joint_set_default;
526
527 xrt_pose root = bone_to_pose(bones[eBone_Root]);
528 xrt_pose wrist = bone_to_pose(bones[eBone_Wrist]);
529
530 // Here we're doing the same transformation as seen in generate_palm_pose_offset.
531 xrt_pose root_inv;
532 math_pose_invert(&root, &root_inv);
533 math_pose_transform(&root_inv, &wrist, &wrist);
534
535 constexpr auto valid_flags = (enum xrt_space_relation_flags)(
536 XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
537 XRT_SPACE_RELATION_POSITION_TRACKED_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
538
539 xrt_pose wrist_xr = wrist;
540
541 switch (skeleton_hand) {
542 case XRT_HAND_LEFT: {
543 math_quat_rotate(&wrist_xr.orientation, &left_wrist_rotate, &wrist_xr.orientation);
544 break;
545 }
546 case XRT_HAND_RIGHT: {
547 math_quat_rotate(&wrist_xr.orientation, &right_wrist_rotate, &wrist_xr.orientation);
548 break;
549 }
550 }
551
552 joints[XRT_HAND_JOINT_WRIST].relation.pose = wrist_xr;
553 joints[XRT_HAND_JOINT_WRIST].relation.relation_flags = valid_flags;
554
555 xrt_pose parent_pose;
556 for (int joint = XRT_HAND_JOINT_THUMB_METACARPAL; joint <= XRT_HAND_JOINT_LITTLE_TIP; ++joint) {
557 // Luckily openvr and openxr joint values match
558 xrt_pose pose = bone_to_pose(bones[joint]);
559 joints[joint].relation.relation_flags = valid_flags;
560
561 if (u_hand_joint_is_metacarpal((xrt_hand_joint)joint)) {
562 // Counter transform metacarpals
563 math_pose_transform(&root, &pose, &pose);
564 math_pose_transform(&wrist, &pose, &pose);
565 } else {
566 math_pose_transform(&parent_pose, &pose, &pose);
567 }
568
569 parent_pose = pose;
570
571 // Rotate joint to OpenXR orientation
572 switch (skeleton_hand) {
573 case XRT_HAND_LEFT: math_quat_rotate(&pose.orientation, &left_hand_rotate, &pose.orientation); break;
574 case XRT_HAND_RIGHT: math_quat_rotate(&pose.orientation, &right_hand_rotate, &pose.orientation); break;
575 }
576 joints[joint].relation.pose = pose;
577 }
578
579 joints[XRT_HAND_JOINT_PALM].relation.relation_flags = valid_flags;
580 joints[XRT_HAND_JOINT_PALM].relation.pose =
581 generate_palm_pose(joints[XRT_HAND_JOINT_MIDDLE_METACARPAL].relation.pose,
582 joints[XRT_HAND_JOINT_MIDDLE_PROXIMAL].relation.pose);
583
584 u_hand_joints_apply_joint_width(&joint_set);
585 this->joint_history.push_back(JointsWithTimestamp{joint_set, timestamp});
586}
587
588xrt_input *
589Device::get_input_from_name(const std::string_view name)
590{
591 static const std::array ignore_inputs = {"/input/finger/index"sv, "/input/finger/middle"sv,
592 "/input/finger/ring"sv, "/input/finger/pinky"sv,
593 "/input/grip/touch"sv};
594
595 // Return nullptr without any other output to suppress a pile of useless warnings found below.
596 if (std::ranges::find(ignore_inputs, name) != std::ranges::end(ignore_inputs)) {
597 return nullptr;
598 }
599 auto input = inputs_map.find(name);
600 if (input == inputs_map.end()) {
601 DEV_WARN("requested unknown input name %s for device %s", std::string(name).c_str(), serial);
602 return nullptr;
603 }
604 return input->second;
605}
606
607void
608ControllerDevice::set_haptic_handle(vr::VRInputComponentHandle_t handle)
609{
610 // this should only be set once
611 assert(output == nullptr);
612 DEV_DEBUG("setting haptic handle for %" PRIu64, handle);
613 haptic_handle = handle;
614 xrt_output_name name;
615 switch (this->name) {
616 case XRT_DEVICE_VIVE_WAND: {
617 name = XRT_OUTPUT_NAME_VIVE_HAPTIC;
618 break;
619 }
620 case XRT_DEVICE_INDEX_CONTROLLER: {
621 name = XRT_OUTPUT_NAME_INDEX_HAPTIC;
622 break;
623 }
624 case XRT_DEVICE_VIVE_TRACKER: {
625 name = XRT_OUTPUT_NAME_VIVE_TRACKER_HAPTIC;
626 break;
627 }
628 default: {
629 DEV_WARN("Unknown device name (%u), haptics will not work", this->name);
630 return;
631 }
632 }
633 output = std::make_unique<xrt_output>(xrt_output{name});
634 this->output_count = 1;
635 this->outputs = output.get();
636}
637
638xrt_result_t
639Device::update_inputs()
640{
641 std::lock_guard<std::mutex> lock(frame_mutex);
642 ctx->maybe_run_frame(++current_frame);
643 return XRT_SUCCESS;
644}
645
646xrt_result_t
647ControllerDevice::get_hand_tracking(enum xrt_input_name name,
648 int64_t desired_timestamp_ns,
649 struct xrt_hand_joint_set *out_value,
650 int64_t *out_timestamp_ns)
651{
652 if (!has_hand_tracking) {
653 return XRT_ERROR_NOT_IMPLEMENTED;
654 }
655
656 // No handtracking data?
657 if (joint_history.empty()) {
658 out_value->is_active = false;
659 return XRT_SUCCESS;
660 }
661
662 const auto it = std::ranges::lower_bound(joint_history, desired_timestamp_ns, {},
663 [](const JointsWithTimestamp &joint) { return joint.timestamp; });
664
665 auto predict_joint_set = [out_value, out_timestamp_ns,
666 desired_timestamp_ns](const JointsWithTimestamp &joints) {
667 out_value->is_active = joints.joint_set.is_active;
668 int64_t delta_ns = desired_timestamp_ns - joints.timestamp;
669 double delta_s = time_ns_to_s(delta_ns);
670
671 *out_timestamp_ns = desired_timestamp_ns;
672 m_predict_relation(&joints.joint_set.hand_pose, delta_s, &out_value->hand_pose);
673 for (int i = 0; i < XRT_HAND_JOINT_COUNT; i++) {
674 auto &new_joint = joints.joint_set.values.hand_joint_set_default[i];
675 auto &interp_joint = out_value->values.hand_joint_set_default[i];
676
677 m_predict_relation(&new_joint.relation, delta_s, &interp_joint.relation);
678 interp_joint.radius = new_joint.radius;
679 }
680 };
681
682 if (it == joint_history.end()) {
683 // Timestamp is newer than anything in history
684 predict_joint_set(joint_history.back());
685 } else if (desired_timestamp_ns == it->timestamp) {
686 *out_value = it->joint_set;
687 *out_timestamp_ns = it->timestamp;
688 } else if (it == joint_history.begin()) {
689 // Timestamp is older than anything in history
690 predict_joint_set(joint_history.front());
691 } else {
692 // Interpolate
693 auto it_previous = it - 1;
694
695 auto delta_before = desired_timestamp_ns - it_previous->timestamp;
696 auto delta_after = it->timestamp - desired_timestamp_ns;
697 float lerp_amt = (float)delta_before / (float)(delta_after - delta_before);
698
699 auto interpolate = [lerp_amt](xrt_space_relation &previous, xrt_space_relation &next,
700 xrt_space_relation &out) {
701 auto flags = (xrt_space_relation_flags)(previous.relation_flags & next.relation_flags);
702 m_space_relation_interpolate(&previous, &next, lerp_amt, flags, &out);
703 };
704
705 interpolate(it_previous->joint_set.hand_pose, it->joint_set.hand_pose, out_value->hand_pose);
706
707 for (int i = 0; i < XRT_HAND_JOINT_COUNT; i++) {
708 auto &prev = it_previous->joint_set.values.hand_joint_set_default[i];
709 auto &next = it->joint_set.values.hand_joint_set_default[i];
710 auto &out = out_value->values.hand_joint_set_default[i];
711 interpolate(prev.relation, next.relation, out.relation);
712 out.radius = next.radius;
713 }
714
715 *out_timestamp_ns = desired_timestamp_ns;
716 }
717
718 return XRT_SUCCESS;
719}
720
721void
722Device::get_pose(uint64_t at_timestamp_ns, xrt_space_relation *out_relation)
723{
724 m_relation_history_get(this->relation_hist, at_timestamp_ns, out_relation);
725}
726
727xrt_result_t
728Device::get_battery_status(bool *out_present, bool *out_charging, float *out_charge)
729{
730 *out_present = this->provides_battery_status;
731 *out_charging = this->charging;
732 *out_charge = this->charge;
733 return XRT_SUCCESS;
734}
735
736xrt_result_t
737HmdDevice::get_brightness(float *out_brightness)
738{
739 *out_brightness = this->brightness;
740 return XRT_SUCCESS;
741}
742
743xrt_result_t
744HmdDevice::set_brightness(float brightness, bool relative)
745{
746 constexpr auto min_brightness = 0.2f;
747 constexpr auto max_brightness = 1.5f;
748
749 const auto target_brightness = relative ? (this->brightness + brightness) : brightness;
750 this->brightness = std::clamp(target_brightness, min_brightness, max_brightness);
751 const auto analog_gain =
752 std::clamp(brightness_to_analog_gain(this->brightness), analog_gain_range.min, analog_gain_range.max);
753 ctx->settings.SetFloat(vr::k_pch_SteamVR_Section, analog_gain_settings_key, analog_gain);
754 return XRT_SUCCESS;
755}
756
757xrt_result_t
758HmdDevice::get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation)
759{
760 switch (name) {
761 case XRT_INPUT_GENERIC_HEAD_POSE: Device::get_pose(at_timestamp_ns, out_relation); break;
762 default: U_LOG_XDEV_UNSUPPORTED_INPUT(this, ctx->log_level, name); return XRT_ERROR_INPUT_UNSUPPORTED;
763 }
764
765 return XRT_SUCCESS;
766}
767
768xrt_result_t
769ControllerDevice::get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation)
770{
771 xrt_space_relation rel = {};
772 Device::get_pose(at_timestamp_ns, &rel);
773
774 xrt_pose pose_offset = XRT_POSE_IDENTITY;
775
776 if (name == XRT_INPUT_GENERIC_PALM_POSE) {
777 if (!palm_offsets[skeleton_hand].has_value()) {
778 DEV_ERR("%s hand skeleton has not been initialized",
779 skeleton_hand == XRT_HAND_LEFT ? "left" : "right");
780 *out_relation = XRT_SPACE_RELATION_ZERO;
781 return XRT_SUCCESS;
782 }
783 pose_offset = *palm_offsets[skeleton_hand];
784 } else {
785 vive_poses_get_pose_offset(input_class->name, device_type, name, &pose_offset);
786 }
787 xrt_relation_chain relchain = {};
788
789 m_relation_chain_push_pose(&relchain, &pose_offset);
790 m_relation_chain_push_relation(&relchain, &rel);
791 m_relation_chain_resolve(&relchain, out_relation);
792
793 struct xrt_pose *p = &out_relation->pose;
794 DEV_DEBUG("controller %u: GET_POSITION (%f %f %f) GET_ORIENTATION (%f, %f, %f, %f)", name, p->position.x,
795 p->position.y, p->position.z, p->orientation.x, p->orientation.y, p->orientation.z, p->orientation.w);
796
797 return XRT_SUCCESS;
798}
799
800xrt_result_t
801ControllerDevice::set_output(xrt_output_name name, const xrt_output_value *value)
802
803{
804 const auto &vib = value->vibration;
805 if (vib.amplitude == 0.0)
806 return XRT_SUCCESS;
807 vr::VREvent_HapticVibration_t event;
808 event.containerHandle = container_handle;
809 event.componentHandle = haptic_handle;
810 event.fDurationSeconds = (float)vib.duration_ns / 1e9f;
811 // 0.0f in OpenXR means let the driver determine a frequency, but
812 // in OpenVR means no haptic, so let's set a reasonable default.
813 float frequency = vib.frequency;
814 if (frequency == 0.0) {
815 frequency = 200.0f;
816 }
817
818 event.fFrequency = frequency;
819 event.fAmplitude = vib.amplitude;
820
821 ctx->add_haptic_event(event);
822 return XRT_SUCCESS;
823}
824
825void
826HmdDevice::SetDisplayEyeToHead(uint32_t unWhichDevice,
827 const vr::HmdMatrix34_t &eyeToHeadLeft,
828 const vr::HmdMatrix34_t &eyeToHeadRight)
829{
830 xrt_matrix_3x3 leftEye_prequat;
831 xrt_matrix_3x3 rightEye_prequat;
832
833 xrt_pose leftEye_postquat;
834 xrt_pose rightEye_postquat;
835
836 // This is a HmdMatrix34 to xrt_matrix_3x3 copy.
837 for (int i = 0; i < 3; ++i) {
838 for (int j = 0; j < 3; ++j) {
839 leftEye_prequat.v[i * 3 + j] = eyeToHeadLeft.m[i][j];
840 rightEye_prequat.v[i * 3 + j] = eyeToHeadRight.m[i][j];
841 }
842 }
843
844 math_quat_from_matrix_3x3(&leftEye_prequat, &leftEye_postquat.orientation);
845 math_quat_from_matrix_3x3(&rightEye_prequat, &rightEye_postquat.orientation);
846 leftEye_postquat.position.x = eyeToHeadLeft.m[0][3];
847 leftEye_postquat.position.y = eyeToHeadLeft.m[1][3];
848 leftEye_postquat.position.z = eyeToHeadLeft.m[2][3];
849
850 rightEye_postquat.position.x = eyeToHeadRight.m[0][3];
851 rightEye_postquat.position.y = eyeToHeadRight.m[1][3];
852 rightEye_postquat.position.z = eyeToHeadRight.m[2][3];
853
854 this->eye[0].orientation = leftEye_postquat.orientation;
855 this->eye[0].position = leftEye_postquat.position;
856 this->eye[1].orientation = rightEye_postquat.orientation;
857 this->eye[1].position = rightEye_postquat.position;
858}
859
860xrt_result_t
861HmdDevice::get_view_poses(const xrt_vec3 *default_eye_relation,
862 uint64_t at_timestamp_ns,
863 xrt_view_type view_type,
864 uint32_t view_count,
865 xrt_space_relation *out_head_relation,
866 xrt_fov *out_fovs,
867 xrt_pose *out_poses)
868{
869 struct xrt_vec3 eye_relation = *default_eye_relation;
870 eye_relation.x = ipd;
871
872 xrt_result_t xret = u_device_get_view_poses( //
873 this, //
874 &eye_relation, //
875 at_timestamp_ns, //
876 view_type, //
877 view_count, //
878 out_head_relation, //
879 out_fovs, //
880 out_poses); //
881 if (xret != XRT_SUCCESS) {
882 return xret;
883 }
884
885 out_poses[0].orientation = this->eye[0].orientation;
886 out_poses[0].position.z = this->eye[0].position.z;
887 out_poses[0].position.y = this->eye[0].position.y;
888 out_poses[1].orientation = this->eye[1].orientation;
889 out_poses[1].position.z = this->eye[1].position.z;
890 out_poses[1].position.y = this->eye[1].position.y;
891
892 return XRT_SUCCESS;
893}
894
895xrt_result_t
896HmdDevice::compute_distortion(uint32_t view, float u, float v, xrt_uv_triplet *out_result)
897{
898 // Vive Pro 2 has a vertically flipped distortion map.
899 if (this->variant == VIVE_VARIANT_PRO2) {
900 v = 1.0f - v;
901 }
902
903 vr::EVREye eye = (view == 0) ? vr::Eye_Left : vr::Eye_Right;
904 vr::DistortionCoordinates_t coords = this->hmd_parts->display->ComputeDistortion(eye, u, v);
905 out_result->r = {coords.rfRed[0], coords.rfRed[1]};
906 out_result->g = {coords.rfGreen[0], coords.rfGreen[1]};
907 out_result->b = {coords.rfBlue[0], coords.rfBlue[1]};
908 return XRT_SUCCESS;
909}
910
911void
912HmdDevice::set_hmd_parts(std::unique_ptr<Parts> parts)
913{
914 {
915 std::lock_guard lk(hmd_parts_mut);
916 hmd_parts = std::move(parts);
917 this->hmd = &hmd_parts->base;
918 }
919 hmd_parts_cv.notify_all();
920}
921
922namespace {
923xrt_quat
924copy_quat(const vr::HmdQuaternion_t &quat)
925{
926 return xrt_quat{(float)quat.x, (float)quat.y, (float)quat.z, (float)quat.w};
927}
928
929xrt_vec3
930copy_vec3(const double (&vec)[3])
931{
932 return xrt_vec3{(float)vec[0], (float)vec[1], (float)vec[2]};
933}
934
935xrt_pose
936copy_pose(const vr::HmdQuaternion_t &orientation, const double (&position)[3])
937{
938 return xrt_pose{copy_quat(orientation), copy_vec3(position)};
939}
940} // namespace
941
942void
943Device::init_chaperone(const std::string &steam_install)
944{
945 static bool initialized = false;
946 if (initialized)
947 return;
948
949 initialized = true;
950
951 // Lighthouse driver seems to create a lighthousedb.json and a chaperone_info.vrchap (which is json)
952 // We will use the known_universes from the lighthousedb.json to match to a universe from chaperone_info.vrchap
953
954 using xrt::auxiliary::util::json::JSONNode;
955 auto lighthousedb = JSONNode::loadFromFile(steam_install + "/config/lighthouse/lighthousedb.json");
956 if (lighthousedb.isInvalid()) {
957 DEV_ERR("Couldn't load lighthousedb file, playspace center will be off - was Room Setup run?");
958 return;
959 }
960 auto chap_info = JSONNode::loadFromFile(steam_install + "/config/chaperone_info.vrchap");
961 if (chap_info.isInvalid()) {
962 DEV_ERR("Couldn't load chaperone info, playspace center will be off - was Room Setup run?");
963 return;
964 }
965
966 JSONNode info = {};
967 bool universe_found = false;
968
969 // XXX: This may be broken if there are multiple known universes - how do we determine which to use then?
970 auto known_universes = lighthousedb["known_universes"].asArray();
971 for (auto &universe : known_universes) {
972 const std::string id = universe["id"].asString();
973 for (const JSONNode &u : chap_info["universes"].asArray()) {
974 if (u["universeID"].asString() == id) {
975 DEV_INFO("Found info for universe %s", id.c_str());
976 info = u;
977 universe_found = true;
978 break;
979 }
980 }
981 if (universe_found) {
982 break;
983 }
984 }
985
986 if (info.isInvalid()) {
987 DEV_ERR("Couldn't find chaperone info for any known universe, playspace center will be off");
988 return;
989 }
990
991 std::vector<JSONNode> translation_arr = info["standing"]["translation"].asArray();
992
993 // If the array is missing elements, add zero.
994 for (size_t i = translation_arr.size(); i < 3; i++) {
995 translation_arr.push_back(JSONNode("0.0"));
996 }
997
998 const double yaw = info["standing"]["yaw"].asDouble();
999 const xrt_vec3 yaw_axis{0.0, -1.0, 0.0};
1000 math_quat_from_angle_vector(static_cast<float>(yaw), &yaw_axis, &chaperone.orientation);
1001 chaperone.position = copy_vec3({
1002 translation_arr[0].asDouble(),
1003 translation_arr[1].asDouble(),
1004 translation_arr[2].asDouble(),
1005 });
1006 math_quat_rotate_vec3(&chaperone.orientation, &chaperone.position, &chaperone.position);
1007 DEV_INFO("Initialized chaperone data.");
1008}
1009
1010inline xrt_space_relation_flags
1011operator|(xrt_space_relation_flags a, xrt_space_relation_flags b)
1012{
1013 return static_cast<xrt_space_relation_flags>(static_cast<uint32_t>(a) | static_cast<uint32_t>(b));
1014}
1015
1016inline xrt_space_relation_flags &
1017operator|=(xrt_space_relation_flags &a, xrt_space_relation_flags b)
1018{
1019 a = a | b;
1020 return a;
1021}
1022
1023void
1024Device::update_pose(const vr::DriverPose_t &newPose) const
1025{
1026 xrt_space_relation relation = {};
1027
1028 if (newPose.poseIsValid) {
1029 // The pose is known to be valid but that alone is not enough to say whether the data comes from
1030 // inference or if it represents actively tracked position and orientation data. Furthermore we avoid
1031 // assumptions regarding the validity of time-derivatives until we know that they're based on tracked
1032 // data. This is a conservative strategy that should reduce concerns regarding drift. see
1033 // https://registry.khronos.org/OpenXR/specs/1.1/man/html/XrSpaceLocationFlagBits.html
1034 relation.relation_flags |= xrt_space_relation_flags::XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
1035 xrt_space_relation_flags::XRT_SPACE_RELATION_POSITION_VALID_BIT;
1036
1037 switch (newPose.result) {
1038 // see https://github.com/ValveSoftware/openvr/blob/master/docs/Driver_API_Documentation.md
1039 case vr::TrackingResult_Running_OK:
1040 // If the tracker is running ok then we have actively tracked 6DoF data
1041 relation.relation_flags |=
1042 xrt_space_relation_flags::XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
1043 xrt_space_relation_flags::XRT_SPACE_RELATION_POSITION_TRACKED_BIT |
1044 xrt_space_relation_flags::XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT |
1045 xrt_space_relation_flags::XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT;
1046 break;
1047 case vr::TrackingResult_Fallback_RotationOnly:
1048 case vr::TrackingResult_Running_OutOfRange:
1049 // If the tracking is degraded we should still be able to assume that we still have tracked 3DoF
1050 // data
1051 relation.relation_flags |=
1052 xrt_space_relation_flags::XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
1053 xrt_space_relation_flags::XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT;
1054 break;
1055 default: break;
1056 }
1057 }
1058
1059 // The driver still outputs good pose data regardless of the pose results above
1060 relation.pose = copy_pose(newPose.qRotation, newPose.vecPosition);
1061 relation.linear_velocity = copy_vec3(newPose.vecVelocity);
1062 relation.angular_velocity = copy_vec3(newPose.vecAngularVelocity);
1063
1064 // local transform (head to driver offset)
1065 const xrt_pose local = copy_pose(newPose.qDriverFromHeadRotation, newPose.vecDriverFromHeadTranslation);
1066
1067 // IMU linear velocity contribution due to rotation around driver origin (tangential velocity)
1068 xrt_vec3 tangential_velocity;
1069 math_vec3_cross(&relation.angular_velocity, &local.position, &tangential_velocity);
1070 math_quat_rotate_vec3(&relation.pose.orientation, &tangential_velocity, &tangential_velocity);
1071 math_vec3_accum(&tangential_velocity, &relation.linear_velocity);
1072
1073 // apply local transform
1074 math_quat_rotate_vec3(&relation.pose.orientation, &relation.angular_velocity, &relation.angular_velocity);
1075 math_pose_transform(&relation.pose, &local, &relation.pose);
1076
1077 // apply world transform
1078 const xrt_pose world = copy_pose(newPose.qWorldFromDriverRotation, newPose.vecWorldFromDriverTranslation);
1079 math_pose_transform(&world, &relation.pose, &relation.pose);
1080 math_quat_rotate_vec3(&world.orientation, &relation.linear_velocity, &relation.linear_velocity);
1081 math_quat_rotate_vec3(&world.orientation, &relation.angular_velocity, &relation.angular_velocity);
1082
1083 // apply chaperone transform
1084 math_pose_transform(&chaperone, &relation.pose, &relation.pose);
1085 math_quat_rotate_vec3(&chaperone.orientation, &relation.linear_velocity, &relation.linear_velocity);
1086 math_quat_rotate_vec3(&chaperone.orientation, &relation.angular_velocity, &relation.angular_velocity);
1087
1088 const uint64_t ts = chrono_timestamp_ns() + static_cast<uint64_t>(newPose.poseTimeOffset * 1000000.0);
1089
1090 m_relation_history_push(relation_hist, &relation, ts);
1091}
1092
1093vr::ETrackedPropertyError
1094Device::handle_properties(const vr::PropertyWrite_t *batch, uint32_t count)
1095{
1096 for (uint32_t i = 0; i < count; ++i) {
1097 vr::ETrackedPropertyError err = handle_property_write(batch[i]);
1098 if (err != vr::ETrackedPropertyError::TrackedProp_Success) {
1099 return err;
1100 }
1101 }
1102 return vr::ETrackedPropertyError::TrackedProp_Success;
1103}
1104
1105vr::ETrackedPropertyError
1106Device::handle_read_properties(vr::PropertyRead_t *batch, uint32_t count)
1107{
1108 for (uint32_t i = 0; i < count; ++i) {
1109 vr::ETrackedPropertyError err = handle_generic_property_read(batch[i]);
1110 if (err != vr::ETrackedPropertyError::TrackedProp_Success) {
1111 return err;
1112 }
1113 }
1114 return vr::ETrackedPropertyError::TrackedProp_Success;
1115}
1116
1117void
1118HmdDevice::set_nominal_frame_interval(uint64_t interval_ns)
1119{
1120 auto set = [this, interval_ns] { hmd_parts->base.screens[0].nominal_frame_interval_ns = interval_ns; };
1121
1122 if (hmd_parts) {
1123 set();
1124 } else {
1125 std::thread t([this, set] {
1126 std::unique_lock lk(hmd_parts_mut);
1127 hmd_parts_cv.wait(lk, [this] { return hmd_parts != nullptr; });
1128 set();
1129 });
1130 t.detach();
1131 }
1132}
1133
1134void
1135HmdDevice::set_scanout_type(xrt_scanout_direction direction, int64_t time_ns)
1136{
1137 auto set = [this, direction, time_ns] {
1138 hmd_parts->base.screens[0].scanout_direction = direction;
1139 hmd_parts->base.screens[0].scanout_time_ns = time_ns;
1140 };
1141
1142 if (hmd_parts) {
1143 set();
1144 } else {
1145 std::thread t([this, set] {
1146 std::unique_lock lk(hmd_parts_mut);
1147 hmd_parts_cv.wait(lk, [this] { return hmd_parts != nullptr; });
1148 set();
1149 });
1150 t.detach();
1151 }
1152}
1153
1154namespace {
1155// From openvr driver documentation
1156// (https://github.com/ValveSoftware/openvr/blob/master/docs/Driver_API_Documentation.md#Input-Profiles):
1157// "Input profiles are expected to be a valid JSON file,
1158// and should be located: <driver_name>/resources/input/<device_name>_profile.json"
1159// So we will just parse the file name to get the device name.
1160std::string_view
1161parse_profile(std::string_view path)
1162{
1163 size_t name_start_idx = path.find_last_of('/') + 1;
1164 size_t name_end_idx = path.find_last_of('_');
1165 return path.substr(name_start_idx, name_end_idx - name_start_idx);
1166}
1167} // namespace
1168
1169vr::ETrackedPropertyError
1170Device::handle_generic_property_write(const vr::PropertyWrite_t &prop)
1171{
1172 switch (prop.writeType) {
1173 case vr::EPropertyWriteType::PropertyWrite_Set:
1174 if (properties.count(prop.prop) > 0) {
1175 Property &p = properties.at(prop.prop);
1176 if (p.tag != prop.unTag) {
1177 return vr::ETrackedPropertyError::TrackedProp_WrongDataType;
1178 }
1179 p.buffer.resize(prop.unBufferSize);
1180 std::memcpy(p.buffer.data(), prop.pvBuffer, prop.unBufferSize);
1181 return vr::ETrackedPropertyError::TrackedProp_Success;
1182 } else {
1183 properties.emplace(std::piecewise_construct, std::forward_as_tuple(prop.prop),
1184 std::forward_as_tuple(prop.unTag, prop.pvBuffer, prop.unBufferSize));
1185 }
1186 break;
1187 case vr::EPropertyWriteType::PropertyWrite_Erase: properties.erase(prop.prop); break;
1188 case vr::EPropertyWriteType::PropertyWrite_SetError:
1189 DEV_DEBUG("Property write type SetError not supported! (property %d)", prop.prop);
1190 break;
1191 }
1192 return vr::ETrackedPropertyError::TrackedProp_Success;
1193}
1194
1195vr::ETrackedPropertyError
1196Device::handle_generic_property_read(vr::PropertyRead_t &prop)
1197{
1198 if (properties.count(prop.prop) == 0) {
1199 // not verified if this is the correct error
1200 return vr::ETrackedPropertyError::TrackedProp_UnknownProperty;
1201 }
1202 Property &p = properties.at(prop.prop);
1203 prop.unTag = p.tag;
1204 prop.unRequiredBufferSize = p.buffer.size();
1205 if (prop.pvBuffer == nullptr || prop.unBufferSize < p.buffer.size()) {
1206 prop.eError = vr::ETrackedPropertyError::TrackedProp_BufferTooSmall;
1207 return prop.eError;
1208 }
1209 std::memcpy(prop.pvBuffer, p.buffer.data(), p.buffer.size());
1210 prop.eError = vr::ETrackedPropertyError::TrackedProp_Success;
1211 return prop.eError;
1212}
1213
1214vr::ETrackedPropertyError
1215Device::handle_property_write(const vr::PropertyWrite_t &prop)
1216{
1217 switch (prop.prop) {
1218 case vr::Prop_ManufacturerName_String: {
1219 this->manufacturer = std::string(static_cast<char *>(prop.pvBuffer), prop.unBufferSize);
1220 if (!this->model.empty()) {
1221 std::snprintf(this->str, std::size(this->str), "%s %s", this->manufacturer.c_str(),
1222 this->model.c_str());
1223 }
1224 break;
1225 }
1226 case vr::Prop_ModelNumber_String: {
1227 this->model = std::string(static_cast<char *>(prop.pvBuffer), prop.unBufferSize);
1228 if (!this->manufacturer.empty()) {
1229 std::snprintf(this->str, std::size(this->str), "%s %s", this->manufacturer.c_str(),
1230 this->model.c_str());
1231 }
1232 break;
1233 }
1234 default: {
1235 DEV_DEBUG("Unhandled property: %i", prop.prop);
1236 break;
1237 }
1238 }
1239 return handle_generic_property_write(prop);
1240}
1241
1242bool
1243HmdDevice::init_vive_pro_2(struct xrt_prober *xp)
1244{
1245 xrt_result_t xret;
1246 int ret = 0;
1247
1248 struct xrt_prober_device **devices = nullptr;
1249 size_t device_count;
1250
1251 xret = xrt_prober_lock_list(xp, &devices, &device_count);
1252 if (xret != XRT_SUCCESS) {
1253 DEV_ERR("Failed to lock prober device list");
1254 return false;
1255 }
1256
1257 for (size_t i = 0; i < device_count; i++) {
1258 struct xrt_prober_device *dev = devices[i];
1259
1260 if (dev->vendor_id == VP2_VID && dev->product_id == VP2_PID) {
1261 DEV_INFO("Found Vive Pro 2 HID device");
1262 struct os_hid_device *hid_dev = nullptr;
1263 ret = xrt_prober_open_hid_interface(xp, dev, 0, &hid_dev);
1264 if (ret != 0) {
1265 DEV_ERR("Failed to open Vive Pro 2 HID interface");
1266 break;
1267 }
1268
1269 ret = vp2_hid_open(hid_dev, &this->vp2.hid);
1270 if (ret != 0) {
1271 DEV_ERR("Failed to open Vive Pro 2 HID device");
1272 break;
1273 }
1274
1275 break;
1276 }
1277 }
1278
1279 xrt_prober_unlock_list(xp, &devices);
1280
1281 int width, height;
1282 vp2_resolution_get_extents(vp2_get_resolution(this->vp2.hid), &width, &height);
1283
1284 for (int i = 0; i < 2; i++) {
1285 this->hmd_parts->base.views[i].display.w_pixels = width / 2;
1286 this->hmd_parts->base.views[i].display.h_pixels = height;
1287
1288 this->hmd_parts->base.views[i].viewport.w_pixels = width / 2;
1289 this->hmd_parts->base.views[i].viewport.h_pixels = height;
1290 }
1291
1292 this->hmd_parts->base.views[1].viewport.x_pixels = width / 2;
1293
1294 this->hmd_parts->base.screens[0].w_pixels = width;
1295 this->hmd_parts->base.screens[0].h_pixels = height;
1296
1297 return ret == 0;
1298}
1299
1300vr::ETrackedPropertyError
1301HmdDevice::handle_property_write(const vr::PropertyWrite_t &prop)
1302{
1303 switch (prop.prop) {
1304 case vr::Prop_ModelNumber_String: {
1305 std::string model_number(static_cast<char *>(prop.pvBuffer), prop.unBufferSize);
1306
1307 this->variant = vive_determine_variant(model_number.c_str());
1308
1309 return Device::handle_property_write(prop);
1310 }
1311 case vr::Prop_DisplayFrequency_Float: {
1312 assert(prop.unBufferSize == sizeof(float));
1313 float freq = *static_cast<float *>(prop.pvBuffer);
1314 int64_t interval_ns = (1.f / freq) * 1e9f;
1315 set_nominal_frame_interval(interval_ns);
1316 if (variant == VIVE_VARIANT_PRO) {
1317 set_scanout_type(XRT_SCANOUT_DIRECTION_TOP_TO_BOTTOM, interval_ns * 1600.0 / 1624.0);
1318 } else if (variant == VIVE_VARIANT_BEYOND) {
1319 set_scanout_type(XRT_SCANOUT_DIRECTION_TOP_TO_BOTTOM, interval_ns * 2544.0 / 2568.0);
1320 } else if (variant == VIVE_VARIANT_PRO2) {
1321 set_scanout_type(XRT_SCANOUT_DIRECTION_TOP_TO_BOTTOM, interval_ns * 2448.0 / 2574.0);
1322 } else {
1323 set_scanout_type(XRT_SCANOUT_DIRECTION_NONE, 0);
1324 }
1325 break;
1326 }
1327 case vr::Prop_UserIpdMeters_Float: {
1328 if (*static_cast<float *>(prop.pvBuffer) != 0) {
1329 ipd = *static_cast<float *>(prop.pvBuffer);
1330 }
1331 break;
1332 }
1333 case vr::Prop_SecondsFromVsyncToPhotons_Float: {
1334 vsync_to_photon_ns = *static_cast<float *>(prop.pvBuffer) * 1e9f;
1335 break;
1336 }
1337 case vr::Prop_DeviceProvidesBatteryStatus_Bool: {
1338 float supported = *static_cast<bool *>(prop.pvBuffer);
1339 this->provides_battery_status = supported;
1340 DEV_DEBUG("Has battery status: HMD: %s", supported ? "true" : "false");
1341 break;
1342 }
1343 case vr::Prop_DeviceIsCharging_Bool: {
1344 float charging = *static_cast<bool *>(prop.pvBuffer);
1345 this->charging = charging;
1346 DEV_DEBUG("Charging: HMD: %s", charging ? "true" : "false");
1347 break;
1348 }
1349 case vr::Prop_DeviceBatteryPercentage_Float: {
1350 float bat = *static_cast<float *>(prop.pvBuffer);
1351 this->charge = bat;
1352 DEV_DEBUG("Battery: HMD: %f", bat);
1353 break;
1354 }
1355 case vr::Prop_DisplaySupportsAnalogGain_Bool: {
1356 this->supported.brightness_control = *static_cast<bool *>(prop.pvBuffer);
1357 break;
1358 }
1359 case vr::Prop_DisplayMinAnalogGain_Float: {
1360 this->analog_gain_range.min = *static_cast<float *>(prop.pvBuffer);
1361 break;
1362 }
1363 case vr::Prop_DisplayMaxAnalogGain_Float: {
1364 this->analog_gain_range.max = *static_cast<float *>(prop.pvBuffer);
1365 break;
1366 }
1367 default: {
1368 return Device::handle_property_write(prop);
1369 }
1370 }
1371 return handle_generic_property_write(prop);
1372}
1373
1374vr::ETrackedPropertyError
1375ControllerDevice::handle_property_write(const vr::PropertyWrite_t &prop)
1376{
1377 switch (prop.prop) {
1378 case vr::Prop_InputProfilePath_String: {
1379 std::string_view profile =
1380 parse_profile(std::string_view(static_cast<char *>(prop.pvBuffer), prop.unBufferSize));
1381 auto input_class = controller_classes.find(profile);
1382 if (input_class == controller_classes.end()) {
1383 DEV_ERR("Could not find input class for controller profile %s", std::string(profile).c_str());
1384 } else {
1385 this->name = input_class->second.name;
1386 set_input_class(&input_class->second);
1387 }
1388 break;
1389 }
1390 case vr::Prop_ModelNumber_String: {
1391 using namespace std::literals::string_view_literals;
1392 vr::PropertyWrite_t fixedProp = prop;
1393 const std::string_view name = {static_cast<char *>(prop.pvBuffer), prop.unBufferSize};
1394 if (name == "SlimeVR Virtual Tracker\0"sv) {
1395 static const InputClass input_class = {
1396 XRT_DEVICE_VIVE_TRACKER, {XRT_INPUT_GENERIC_TRACKER_POSE}, {}};
1397 this->name = input_class.name;
1398 set_input_class(&input_class);
1399 this->manufacturer = name.substr(0, name.find_first_of(' '));
1400 fixedProp.pvBuffer = (char *)fixedProp.pvBuffer + this->manufacturer.size() +
1401 (this->manufacturer.size() != name.size());
1402 fixedProp.unBufferSize = name.end() - (char *)fixedProp.pvBuffer;
1403 }
1404 return Device::handle_property_write(fixedProp);
1405 }
1406 case vr::Prop_ControllerRoleHint_Int32: {
1407 vr::ETrackedControllerRole role = *static_cast<vr::ETrackedControllerRole *>(prop.pvBuffer);
1408 switch (role) {
1409 case vr::TrackedControllerRole_Invalid: {
1410 this->device_type = XRT_DEVICE_TYPE_ANY_HAND_CONTROLLER;
1411 break;
1412 }
1413 case vr::TrackedControllerRole_RightHand: {
1414 this->device_type = XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER;
1415 set_active_hand(XRT_HAND_RIGHT);
1416 break;
1417 }
1418 case vr::TrackedControllerRole_LeftHand: {
1419 this->device_type = XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER;
1420 set_active_hand(XRT_HAND_LEFT);
1421 break;
1422 }
1423 case vr::TrackedControllerRole_OptOut: {
1424 this->device_type = XRT_DEVICE_TYPE_GENERIC_TRACKER;
1425 break;
1426 }
1427 default: {
1428 this->device_type = XRT_DEVICE_TYPE_UNKNOWN;
1429 DEV_WARN("requested unimplemented role hint %i", this->device_type);
1430 break;
1431 }
1432 }
1433 break;
1434 }
1435 case vr::Prop_DeviceProvidesBatteryStatus_Bool: {
1436 float supported = *static_cast<bool *>(prop.pvBuffer);
1437 const char *name;
1438 switch (this->device_type) {
1439 case XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER: {
1440 name = "Left";
1441 break;
1442 }
1443 case XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER: {
1444 name = "Right";
1445 break;
1446 }
1447 default: {
1448 name = "Unknown";
1449 break;
1450 }
1451 }
1452 this->provides_battery_status = supported;
1453 DEV_DEBUG("Has battery status: %s: %s", name, supported ? "true" : "false");
1454 break;
1455 }
1456 case vr::Prop_DeviceIsCharging_Bool: {
1457 float charging = *static_cast<bool *>(prop.pvBuffer);
1458 const char *name;
1459 switch (this->device_type) {
1460 case XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER: {
1461 name = "Left";
1462 break;
1463 }
1464 case XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER: {
1465 name = "Right";
1466 break;
1467 }
1468 default: {
1469 name = "Unknown";
1470 }
1471 }
1472 this->charging = charging;
1473 DEV_DEBUG("Charging: %s: %s", name, charging ? "true" : "false");
1474 break;
1475 }
1476 case vr::Prop_DeviceBatteryPercentage_Float: {
1477 float bat = *static_cast<float *>(prop.pvBuffer);
1478 const char *name;
1479 switch (this->device_type) {
1480 case XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER: {
1481 name = "Left";
1482 break;
1483 }
1484 case XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER: {
1485 name = "Right";
1486 break;
1487 }
1488 default: {
1489 name = "Unknown";
1490 }
1491 }
1492 this->charge = bat;
1493 DEV_DEBUG("Battery: %s: %f", name, bat);
1494 break;
1495 }
1496 default: {
1497 return Device::handle_property_write(prop);
1498 }
1499 }
1500 return handle_generic_property_write(prop);
1501}