The open source OpenXR runtime
matrixfurry.com
1// Copyright 2021, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Combination of multiple @ref xrt_device.
6 * @author Christoph Haag <christoph.haag@collabora.com>
7 * @ingroup drv_multi
8 */
9
10#include "math/m_api.h"
11#include "math/m_space.h"
12
13#include "util/u_misc.h"
14#include "util/u_debug.h"
15#include "util/u_device.h"
16
17#include "multi.h"
18
19DEBUG_GET_ONCE_LOG_OPTION(multi_log, "MULTI_LOG", U_LOGGING_WARN)
20
21#define MULTI_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__)
22#define MULTI_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__)
23#define MULTI_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__)
24#define MULTI_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__)
25#define MULTI_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__)
26
27struct multi_device
28{
29 struct xrt_device base;
30 enum u_logging_level log_level;
31
32 struct
33 {
34 struct xrt_device *target;
35 struct xrt_device *tracker;
36 enum xrt_input_name input_name;
37 struct xrt_pose offset_inv;
38 } tracking_override;
39
40 enum xrt_tracking_override_type override_type;
41};
42
43static void
44direct_override(struct multi_device *d,
45 struct xrt_space_relation *tracker_relation,
46 struct xrt_space_relation *out_relation)
47{
48 struct xrt_relation_chain xrc = {0};
49 m_relation_chain_push_pose_if_not_identity(&xrc, &d->tracking_override.offset_inv);
50 m_relation_chain_push_relation(&xrc, tracker_relation);
51 m_relation_chain_resolve(&xrc, out_relation);
52}
53
54static void
55attached_override(struct multi_device *d,
56 struct xrt_space_relation *target_relation,
57 struct xrt_pose *target_offset,
58 struct xrt_space_relation *tracker_relation,
59 struct xrt_pose *tracker_offset,
60 struct xrt_space_relation *in_target_space,
61 struct xrt_space_relation *out_relation)
62{
63 /* Example:
64 * - target: hand tracking xrt_device
65 * - tracker: positional tracker that the target is physically attached to
66 * - in_target_space: a tracked hand, relative to target's tracking origin
67 */
68
69 // XXX TODO tracking origin offsets
70 // m_relation_chain_push_inverted_pose_if_not_identity(&xrc, tracker_offset);
71 // m_relation_chain_push_pose_if_not_identity(&xrc, target_offset);
72
73 struct xrt_relation_chain xrc = {0};
74 m_relation_chain_push_relation(&xrc, target_relation);
75 m_relation_chain_push_pose_if_not_identity(&xrc, &d->tracking_override.offset_inv);
76 m_relation_chain_push_relation(&xrc, tracker_relation);
77 m_relation_chain_push_relation(&xrc, in_target_space);
78 m_relation_chain_resolve(&xrc, out_relation);
79}
80
81static xrt_result_t
82get_tracked_pose(struct xrt_device *xdev,
83 enum xrt_input_name name,
84 int64_t at_timestamp_ns,
85 struct xrt_space_relation *out_relation)
86{
87 struct multi_device *d = (struct multi_device *)xdev;
88 struct xrt_device *tracker = d->tracking_override.tracker;
89 enum xrt_input_name tracker_input_name = d->tracking_override.input_name;
90
91 struct xrt_space_relation tracker_relation;
92
93 xrt_result_t xret =
94 xrt_device_get_tracked_pose(tracker, tracker_input_name, at_timestamp_ns, &tracker_relation);
95 if (xret != XRT_SUCCESS) {
96 return xret;
97 }
98
99 switch (d->override_type) {
100 case XRT_TRACKING_OVERRIDE_DIRECT: {
101 direct_override(d, &tracker_relation, out_relation);
102 } break;
103 case XRT_TRACKING_OVERRIDE_ATTACHED: {
104 struct xrt_device *target = d->tracking_override.target;
105
106 struct xrt_space_relation target_relation;
107 xret = xrt_device_get_tracked_pose(target, name, at_timestamp_ns, &target_relation);
108 if (xret != XRT_SUCCESS) {
109 break;
110 }
111
112 // just use the origin of the tracker space as reference frame
113 struct xrt_space_relation in_target_space;
114 m_space_relation_ident(&in_target_space);
115 in_target_space.relation_flags = tracker_relation.relation_flags;
116
117 struct xrt_pose *target_offset = &d->tracking_override.target->tracking_origin->initial_offset;
118 struct xrt_pose *tracker_offset = &d->tracking_override.tracker->tracking_origin->initial_offset;
119
120 attached_override(d, &target_relation, target_offset, &tracker_relation, tracker_offset,
121 &in_target_space, out_relation);
122 } break;
123 }
124
125 return xret;
126}
127
128static void
129destroy(struct xrt_device *xdev)
130{
131 struct multi_device *d = (struct multi_device *)xdev;
132
133 xrt_device_destroy(&d->tracking_override.target);
134
135 // we replaced the target device with us, but no the tracker
136 // xrt_device_destroy(&d->tracking_override.tracker);
137
138 free(d);
139}
140
141static xrt_result_t
142get_hand_tracking(struct xrt_device *xdev,
143 enum xrt_input_name name,
144 int64_t at_timestamp_ns,
145 struct xrt_hand_joint_set *out_value,
146 int64_t *out_timestamp_ns)
147{
148 struct multi_device *d = (struct multi_device *)xdev;
149 struct xrt_device *target = d->tracking_override.target;
150 xrt_result_t xret = xrt_device_get_hand_tracking(target, name, at_timestamp_ns, out_value, out_timestamp_ns);
151 U_LOG_CHK_AND_RET(d->log_level, xret, "xrt_device_get_hand_tracking");
152
153 if (!out_value->is_active) {
154 return XRT_SUCCESS;
155 }
156
157 struct xrt_device *tracker = d->tracking_override.tracker;
158 struct xrt_space_relation tracker_relation;
159 xret =
160 xrt_device_get_tracked_pose(tracker, d->tracking_override.input_name, *out_timestamp_ns, &tracker_relation);
161 U_LOG_CHK_AND_RET(d->log_level, xret, "xrt_device_get_hand_tracking");
162
163 switch (d->override_type) {
164 case XRT_TRACKING_OVERRIDE_DIRECT: direct_override(d, &tracker_relation, &out_value->hand_pose); break;
165 case XRT_TRACKING_OVERRIDE_ATTACHED: {
166
167 // struct xrt_space_relation target_relation;
168 // xrt_device_get_tracked_pose(target, name, at_timestamp_ns, &target_relation);
169
170
171 // just use the origin of the tracker space as reference frame
172 struct xrt_space_relation in_target_space;
173 m_space_relation_ident(&in_target_space);
174 in_target_space.relation_flags = tracker_relation.relation_flags;
175
176 struct xrt_pose *target_offset = &d->tracking_override.target->tracking_origin->initial_offset;
177 struct xrt_pose *tracker_offset = &d->tracking_override.tracker->tracking_origin->initial_offset;
178
179 attached_override(d, &out_value->hand_pose, target_offset, &tracker_relation, tracker_offset,
180 &in_target_space, &out_value->hand_pose);
181 } break;
182 }
183
184 return XRT_SUCCESS;
185}
186
187static xrt_result_t
188set_output(struct xrt_device *xdev, enum xrt_output_name name, const struct xrt_output_value *value)
189{
190 struct multi_device *d = (struct multi_device *)xdev;
191 struct xrt_device *target = d->tracking_override.target;
192 return xrt_device_set_output(target, name, value);
193}
194
195static xrt_result_t
196get_view_poses(struct xrt_device *xdev,
197 const struct xrt_vec3 *default_eye_relation,
198 int64_t at_timestamp_ns,
199 enum xrt_view_type view_type,
200 uint32_t view_count,
201 struct xrt_space_relation *out_head_relation,
202 struct xrt_fov *out_fovs,
203 struct xrt_pose *out_poses)
204{
205 struct multi_device *d = (struct multi_device *)xdev;
206 struct xrt_device *target = d->tracking_override.target;
207
208 xrt_result_t xret = xrt_device_get_view_poses( //
209 target, //
210 default_eye_relation, //
211 at_timestamp_ns, //
212 view_type, //
213 view_count, //
214 out_head_relation, //
215 out_fovs, //
216 out_poses); //
217 if (xret != XRT_SUCCESS) {
218 return xret;
219 }
220
221 /*
222 * Use xrt_device_ function to be sure it is exactly
223 * like if the state-tracker called this function.
224 */
225 return xrt_device_get_tracked_pose(xdev, XRT_INPUT_GENERIC_HEAD_POSE, at_timestamp_ns, out_head_relation);
226}
227
228static xrt_result_t
229compute_distortion(struct xrt_device *xdev, uint32_t view, float u, float v, struct xrt_uv_triplet *result)
230{
231 struct multi_device *d = (struct multi_device *)xdev;
232 struct xrt_device *target = d->tracking_override.target;
233 return target->compute_distortion(target, view, u, v, result);
234}
235
236static xrt_result_t
237update_inputs(struct xrt_device *xdev)
238{
239 struct multi_device *d = (struct multi_device *)xdev;
240 struct xrt_device *target = d->tracking_override.target;
241 return xrt_device_update_inputs(target);
242}
243
244
245struct xrt_device *
246multi_create_tracking_override(enum xrt_tracking_override_type override_type,
247 struct xrt_device *tracking_override_target,
248 struct xrt_device *tracking_override_tracker,
249 enum xrt_input_name tracking_override_input_name,
250 struct xrt_pose *offset)
251{
252 struct multi_device *d = U_TYPED_CALLOC(struct multi_device);
253
254 if (!d) {
255 return NULL;
256 }
257
258 d->log_level = debug_get_log_option_multi_log();
259 d->override_type = override_type;
260
261 // mimic the tracking override target
262 d->base = *tracking_override_target;
263
264 // but take orientation and position tracking capabilities from tracker
265 d->base.supported.orientation_tracking = tracking_override_tracker->supported.orientation_tracking;
266 d->base.supported.position_tracking = tracking_override_tracker->supported.position_tracking;
267
268 // because we use the tracking data of the tracker, we use its tracking origin instead
269 d->base.tracking_origin = tracking_override_tracker->tracking_origin;
270
271 // The offset describes the physical pose of the tracker in the space of the thing we want to track.
272 // For a tracker that is physically attached at y=.1m to the tracked thing, when querying the pose for the
273 // tracked thing, we want to transform its pose by y-=.1m relative to the tracker. Multiple target devices may
274 // share a single tracker, therefore we cannot simply adjust the tracker's tracking origin.
275 math_pose_invert(offset, &d->tracking_override.offset_inv);
276
277 d->tracking_override.target = tracking_override_target;
278 d->tracking_override.tracker = tracking_override_tracker;
279 d->tracking_override.input_name = tracking_override_input_name;
280
281 d->base.get_tracked_pose = get_tracked_pose;
282 d->base.destroy = destroy;
283 d->base.get_hand_tracking = get_hand_tracking;
284 d->base.set_output = set_output;
285 d->base.update_inputs = update_inputs;
286 d->base.compute_distortion = compute_distortion;
287 d->base.get_view_poses = get_view_poses;
288
289 return &d->base;
290}