matrixfurry.com
The open source OpenXR runtime
1// Copyright 2023, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief OpenVR tracking source.
6 * @author Mateo de Mayo <mateo.demayo@collabora.com>
7 * @ingroup aux_tracking
8 */
9
10#include "t_openvr_tracker.h"
11
12#include "util/u_logging.h"
13#include "xrt/xrt_config_have.h"
14
15#ifdef XRT_HAVE_OPENVR
16#include <openvr.h>
17#include <cstddef>
18#include <map>
19
20#include "math/m_api.h"
21#include "os/os_threading.h"
22#include "util/u_logging.h"
23#include "util/u_time.h"
24#include "xrt/xrt_defines.h"
25#include "xrt/xrt_tracking.h"
26
27struct openvr_tracker
28{
29 vr::IVRSystem *vr_system;
30 struct os_thread_helper thread;
31 std::map<enum openvr_device, struct xrt_pose_sink *> sinks;
32 double sample_frequency_hz;
33
34 struct xrt_pose_sink *
35 get_sink_for_device_index(uint64_t i)
36 {
37 vr::ETrackedDeviceClass dev_class = vr_system->GetTrackedDeviceClass(i);
38 struct xrt_pose_sink *sink = nullptr;
39 if (dev_class == vr::TrackedDeviceClass_HMD && sinks.count(T_OPENVR_DEVICE_HMD) > 0) {
40 sink = sinks.at(T_OPENVR_DEVICE_HMD);
41 } else if (dev_class == vr::TrackedDeviceClass_Controller &&
42 vr_system->GetControllerRoleForTrackedDeviceIndex(i) == vr::TrackedControllerRole_LeftHand &&
43 sinks.count(T_OPENVR_DEVICE_LEFT_CONTROLLER) > 0) {
44 sink = sinks.at(T_OPENVR_DEVICE_LEFT_CONTROLLER);
45 } else if (dev_class == vr::TrackedDeviceClass_Controller &&
46 vr_system->GetControllerRoleForTrackedDeviceIndex(i) ==
47 vr::TrackedControllerRole_RightHand &&
48 sinks.count(T_OPENVR_DEVICE_RIGHT_CONTROLLER) > 0) {
49 sink = sinks.at(T_OPENVR_DEVICE_RIGHT_CONTROLLER);
50 } else if (dev_class == vr::TrackedDeviceClass_GenericTracker &&
51 sinks.count(T_OPENVR_DEVICE_TRACKER) > 0) {
52 sink = sinks.at(T_OPENVR_DEVICE_TRACKER);
53 }
54 return sink;
55 }
56};
57
58static void *
59tracking_loop(void *ot_ptr)
60{
61 struct openvr_tracker *ovrt = (struct openvr_tracker *)ot_ptr;
62
63 while (os_thread_helper_is_running(&ovrt->thread)) {
64 os_nanosleep(U_TIME_1S_IN_NS / ovrt->sample_frequency_hz);
65
66 // Flush events
67 vr::VREvent_t event;
68 while (ovrt->vr_system->PollNextEvent(&event, sizeof(event))) {
69 }
70
71 timepoint_ns now = os_monotonic_get_ns();
72
73 const uint32_t MAX_DEVS = vr::k_unMaxTrackedDeviceCount;
74 auto origin = vr::ETrackingUniverseOrigin::TrackingUniverseRawAndUncalibrated;
75 vr::TrackedDevicePose_t poses[MAX_DEVS];
76 ovrt->vr_system->GetDeviceToAbsoluteTrackingPose(origin, 0, poses, MAX_DEVS);
77
78 for (uint32_t i = 0; i < MAX_DEVS; i++) {
79 struct xrt_pose_sink *sink_for_i = ovrt->get_sink_for_device_index(i);
80 if (sink_for_i != nullptr && poses[i].bDeviceIsConnected && poses[i].bPoseIsValid) {
81 const auto &m = poses[i].mDeviceToAbsoluteTracking.m;
82 struct xrt_vec3 p = {m[0][3], m[1][3], m[2][3]};
83 struct xrt_matrix_3x3 R = {
84 m[0][0], m[0][1], m[0][2], //
85 m[1][0], m[1][1], m[1][2], //
86 m[2][0], m[2][1], m[2][2], //
87 };
88 struct xrt_quat q = {};
89 math_quat_from_matrix_3x3(&R, &q);
90
91 struct xrt_pose_sample sample = {now, {q, p}};
92 xrt_sink_push_pose(sink_for_i, &sample);
93 }
94 }
95 }
96
97 return nullptr;
98}
99
100extern "C" {
101
102struct openvr_tracker *
103t_openvr_tracker_create(double sample_frequency, enum openvr_device *devs, struct xrt_pose_sink **sinks, int sink_count)
104{
105 struct openvr_tracker *ovrt = new openvr_tracker{};
106 os_thread_helper_init(&ovrt->thread);
107
108 for (int i = 0; i < sink_count; i++) {
109 ovrt->sinks[devs[i]] = sinks[i];
110 }
111 ovrt->sample_frequency_hz = sample_frequency;
112
113 vr::EVRInitError e = vr::VRInitError_None;
114 ovrt->vr_system = vr::VR_Init(&e, vr::VRApplication_Background);
115 if (e != vr::VRInitError_None) {
116 if (e == vr::VRInitError_Init_NoServerForBackgroundApp) {
117 U_LOG_E("Unable to find OpenVR server running. error=%d", e);
118 } else {
119 U_LOG_E("Unable to initialize OpenVR, error=%d", e);
120 }
121 return nullptr;
122 }
123 U_LOG(U_LOGGING_INFO, "OpenVR tracker created");
124 return ovrt;
125}
126
127void
128t_openvr_tracker_start(struct openvr_tracker *ovrt)
129{
130 os_thread_helper_start(&ovrt->thread, tracking_loop, ovrt);
131}
132
133void
134t_openvr_tracker_stop(struct openvr_tracker *ovrt)
135{
136 os_thread_helper_stop_and_wait(&ovrt->thread);
137}
138
139void
140t_openvr_tracker_destroy(struct openvr_tracker *ovrt)
141{
142 if (os_thread_helper_is_running(&ovrt->thread)) {
143 t_openvr_tracker_stop(ovrt);
144 }
145 vr::VR_Shutdown();
146 ovrt->vr_system = nullptr;
147 os_thread_helper_destroy(&ovrt->thread);
148 delete ovrt;
149}
150}
151
152#else
153
154struct openvr_tracker *
155t_openvr_tracker_create(double /*unused*/,
156 enum openvr_device * /*unused*/,
157 struct xrt_pose_sink ** /*unused*/,
158 int /*unused*/)
159{
160 U_LOG_W("OpenVR was not built, unable to initialize lighthouse tracking.");
161 return nullptr;
162}
163
164void
165t_openvr_tracker_start(struct openvr_tracker * /*unused*/)
166{}
167
168void
169t_openvr_tracker_stop(struct openvr_tracker * /*unused*/)
170{}
171
172void
173t_openvr_tracker_destroy(struct openvr_tracker * /*unused*/)
174{}
175#endif