matrixfurry.com
The open source OpenXR runtime
1// Copyright 2019-2025, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Misc helpers for device drivers.
6 * @author Jakob Bornecrantz <jakob@collabora.com>
7 * @author Rylie Pavlik <rylie.pavlik@collabora.com>
8 * @author Moshi Turner <moshiturner@protonmail.com>
9 * @author Simon Zeni <simon.zeni@collabora.com>
10 * @ingroup aux_util
11 */
12
13#include "util/u_device.h"
14#include "util/u_device_ni.h"
15#include "util/u_logging.h"
16#include "util/u_misc.h"
17#include "util/u_visibility_mask.h"
18
19#include "math/m_mathinclude.h"
20#include "math/m_api.h"
21
22#include <math.h>
23#include <stdio.h>
24#include <stdlib.h>
25#include <assert.h>
26#include <limits.h>
27
28
29/*
30 *
31 * Matrices.
32 *
33 */
34
35const struct xrt_matrix_2x2 u_device_rotation_right = {{
36 .vecs =
37 {
38 {0, 1},
39 {-1, 0},
40 },
41}};
42
43
44const struct xrt_matrix_2x2 u_device_rotation_left = {{
45 .vecs =
46 {
47 {0, -1},
48 {1, 0},
49 },
50}};
51
52const struct xrt_matrix_2x2 u_device_rotation_ident = {{
53 .vecs =
54 {
55 {1, 0},
56 {0, 1},
57 },
58}};
59
60const struct xrt_matrix_2x2 u_device_rotation_180 = {{
61 .vecs =
62 {
63 {-1, 0},
64 {0, -1},
65 },
66}};
67
68
69/*
70 *
71 * Print helpers.
72 *
73 */
74
75#define PRINT_STR(name, val) U_LOG_RAW("\t%s = %s", name, val)
76
77#define PRINT_INT(name, val) U_LOG_RAW("\t%s = %u", name, val)
78
79#define PRINT_MM(name, val) \
80 U_LOG_RAW("\t%s = %f (%i.%02imm)", name, val, (int32_t)(val * 1000.f), abs((int32_t)(val * 100000.f)) % 100)
81
82#define PRINT_ANGLE(name, val) U_LOG_RAW("\t%s = %f (%i°)", name, val, (int32_t)(val * (180 / M_PI)))
83
84#define PRINT_MAT2X2(name, rot) U_LOG_RAW("\t%s = {%f, %f} {%f, %f}", name, rot.v[0], rot.v[1], rot.v[2], rot.v[3])
85
86/*!
87 * Dump the device config to stderr.
88 */
89void
90u_device_dump_config(struct xrt_device *xdev, const char *prefix, const char *prod)
91{
92 U_LOG_RAW("%s - device_setup", prefix);
93 PRINT_STR("prod", prod);
94 if (xdev->hmd != NULL) {
95 PRINT_INT("screens[0].w_pixels ", xdev->hmd->screens[0].w_pixels);
96 PRINT_INT("screens[0].h_pixels ", xdev->hmd->screens[0].h_pixels);
97 // PRINT_MM( "info.display.w_meters", info.display.w_meters);
98 // PRINT_MM( "info.display.h_meters", info.display.h_meters);
99
100 uint32_t view_count = xdev->hmd->view_count;
101 PRINT_INT("view_count", view_count);
102 for (uint32_t i = 0; i < view_count; ++i) {
103 struct xrt_view *view = &xdev->hmd->views[i];
104 struct xrt_fov *fov = &xdev->hmd->distortion.fov[i];
105 U_LOG_RAW("\tview index = %u", i);
106 U_LOG_RAW("\tviews[%d].viewport.x_pixels = %u", i, view->viewport.x_pixels);
107 U_LOG_RAW("\tviews[%d].viewport.y_pixels = %u", i, view->viewport.y_pixels);
108 U_LOG_RAW("\tviews[%d].viewport.w_pixels = %u", i, view->viewport.w_pixels);
109 U_LOG_RAW("\tviews[%d].viewport.h_pixels = %u", i, view->viewport.h_pixels);
110 U_LOG_RAW("\tviews[%d].display.w_pixels = %u", i, view->display.w_pixels);
111 U_LOG_RAW("\tviews[%d].display.h_pixels = %u", i, view->display.h_pixels);
112 U_LOG_RAW("\tviews[%d].rot = {%f, %f} {%f, %f}", i, view->rot.v[0], view->rot.v[1],
113 view->rot.v[2], view->rot.v[3]);
114 U_LOG_RAW("\tdistortion.fov[%d].angle_left = %f (%i°)", i, fov->angle_left,
115 (int32_t)(fov->angle_left * (180 / M_PI)));
116 U_LOG_RAW("\tdistortion.fov[%d].angle_right = %f (%i°)", i, fov->angle_right,
117 (int32_t)(fov->angle_right * (180 / M_PI)));
118 U_LOG_RAW("\tdistortion.fov[%d].angle_up = %f (%i°)", i, fov->angle_up,
119 (int32_t)(fov->angle_up * (180 / M_PI)));
120 U_LOG_RAW("\tdistortion.fov[%d].angle_down = %f (%i°)", i, fov->angle_down,
121 (int32_t)(fov->angle_down * (180 / M_PI)));
122 }
123 }
124}
125
126
127/*
128 *
129 * Helper setup functions.
130 *
131 */
132
133bool
134u_extents_2d_split_side_by_side(struct xrt_device *xdev, const struct u_extents_2d *extents)
135{
136 uint32_t eye_w_pixels = extents->w_pixels / 2;
137 uint32_t eye_h_pixels = extents->h_pixels;
138
139 xdev->hmd->screens[0].w_pixels = extents->w_pixels;
140 xdev->hmd->screens[0].h_pixels = extents->h_pixels;
141
142 // Left
143 xdev->hmd->views[0].display.w_pixels = eye_w_pixels;
144 xdev->hmd->views[0].display.h_pixels = eye_h_pixels;
145 xdev->hmd->views[0].viewport.x_pixels = 0;
146 xdev->hmd->views[0].viewport.y_pixels = 0;
147 xdev->hmd->views[0].viewport.w_pixels = eye_w_pixels;
148 xdev->hmd->views[0].viewport.h_pixels = eye_h_pixels;
149 xdev->hmd->views[0].rot = u_device_rotation_ident;
150
151 // Right
152 xdev->hmd->views[1].display.w_pixels = eye_w_pixels;
153 xdev->hmd->views[1].display.h_pixels = eye_h_pixels;
154 xdev->hmd->views[1].viewport.x_pixels = eye_w_pixels;
155 xdev->hmd->views[1].viewport.y_pixels = 0;
156 xdev->hmd->views[1].viewport.w_pixels = eye_w_pixels;
157 xdev->hmd->views[1].viewport.h_pixels = eye_h_pixels;
158 xdev->hmd->views[1].rot = u_device_rotation_ident;
159 return true;
160}
161
162bool
163u_device_setup_one_eye(struct xrt_device *xdev, const struct u_device_simple_info *info)
164{
165 uint32_t w_pixels = info->display.w_pixels;
166 uint32_t h_pixels = info->display.h_pixels;
167 float w_meters = info->display.w_meters;
168 float h_meters = info->display.h_meters;
169
170 float lens_center_x_meters = w_meters / 2.0;
171
172 float lens_center_y_meters = info->lens_vertical_position_meters;
173
174 // Common
175 size_t idx = 0;
176 xdev->hmd->blend_modes[idx++] = XRT_BLEND_MODE_OPAQUE;
177 xdev->hmd->blend_mode_count = idx;
178
179 if (xdev->hmd->distortion.models == 0) {
180 xdev->hmd->distortion.models = XRT_DISTORTION_MODEL_NONE;
181 xdev->hmd->distortion.preferred = XRT_DISTORTION_MODEL_NONE;
182 }
183 xdev->hmd->screens[0].w_pixels = info->display.w_pixels;
184 xdev->hmd->screens[0].h_pixels = info->display.h_pixels;
185
186 // Left
187 xdev->hmd->views[0].display.w_pixels = w_pixels;
188 xdev->hmd->views[0].display.h_pixels = h_pixels;
189 xdev->hmd->views[0].viewport.x_pixels = 0;
190 xdev->hmd->views[0].viewport.y_pixels = 0;
191 xdev->hmd->views[0].viewport.w_pixels = w_pixels;
192 xdev->hmd->views[0].viewport.h_pixels = h_pixels;
193 xdev->hmd->views[0].rot = u_device_rotation_ident;
194
195 {
196 /* left eye */
197 if (!math_compute_fovs(w_meters, lens_center_x_meters, info->fov[0], h_meters, lens_center_y_meters, 0,
198 &xdev->hmd->distortion.fov[0])) {
199 return false;
200 }
201 }
202
203 return true;
204}
205
206bool
207u_device_setup_split_side_by_side(struct xrt_device *xdev, const struct u_device_simple_info *info)
208{
209 // 1 or 2 views supported.
210 assert(xdev->hmd->view_count > 0);
211 assert(xdev->hmd->view_count <= 2);
212 assert(xdev->hmd->view_count <= XRT_MAX_VIEWS);
213
214 uint32_t view_count = xdev->hmd->view_count;
215
216 uint32_t w_pixels = info->display.w_pixels / view_count;
217 uint32_t h_pixels = info->display.h_pixels;
218 float w_meters = info->display.w_meters / view_count;
219 float h_meters = info->display.h_meters;
220
221 float lens_center_x_meters[2] = {
222 w_meters - info->lens_horizontal_separation_meters / 2.0f,
223 info->lens_horizontal_separation_meters / 2.0f,
224 };
225
226 float lens_center_y_meters[2] = {
227 info->lens_vertical_position_meters,
228 info->lens_vertical_position_meters,
229 };
230
231 // Common
232 size_t idx = 0;
233 xdev->hmd->blend_modes[idx++] = XRT_BLEND_MODE_OPAQUE;
234 xdev->hmd->blend_mode_count = idx;
235
236 if (xdev->hmd->distortion.models == 0) {
237 xdev->hmd->distortion.models = XRT_DISTORTION_MODEL_NONE;
238 xdev->hmd->distortion.preferred = XRT_DISTORTION_MODEL_NONE;
239 }
240 xdev->hmd->screens[0].w_pixels = info->display.w_pixels;
241 xdev->hmd->screens[0].h_pixels = info->display.h_pixels;
242
243 // Left
244 for (uint32_t i = 0; i < view_count; ++i) {
245 xdev->hmd->views[i].display.w_pixels = w_pixels;
246 xdev->hmd->views[i].display.h_pixels = h_pixels;
247 xdev->hmd->views[i].viewport.x_pixels = w_pixels * i;
248 xdev->hmd->views[i].viewport.y_pixels = 0;
249 xdev->hmd->views[i].viewport.w_pixels = w_pixels;
250 xdev->hmd->views[i].viewport.h_pixels = h_pixels;
251 xdev->hmd->views[i].rot = u_device_rotation_ident;
252 }
253
254 {
255 /* right eye */
256 if (!math_compute_fovs(w_meters, lens_center_x_meters[view_count - 1], info->fov[view_count - 1],
257 h_meters, lens_center_y_meters[view_count - 1], 0,
258 &xdev->hmd->distortion.fov[view_count - 1])) {
259 return false;
260 }
261 }
262 if (view_count == 2) {
263 /* left eye - mirroring right eye */
264 xdev->hmd->distortion.fov[0].angle_up = xdev->hmd->distortion.fov[1].angle_up;
265 xdev->hmd->distortion.fov[0].angle_down = xdev->hmd->distortion.fov[1].angle_down;
266
267 xdev->hmd->distortion.fov[0].angle_left = -xdev->hmd->distortion.fov[1].angle_right;
268 xdev->hmd->distortion.fov[0].angle_right = -xdev->hmd->distortion.fov[1].angle_left;
269 }
270
271 return true;
272}
273
274void *
275u_device_allocate(enum u_device_alloc_flags flags, size_t size, size_t input_count, size_t output_count)
276{
277 bool alloc_hmd = (flags & U_DEVICE_ALLOC_HMD) != 0;
278 bool alloc_tracking = (flags & U_DEVICE_ALLOC_TRACKING_NONE) != 0;
279
280 size_t total_size = size;
281
282 // Inputs
283 size_t offset_inputs = total_size;
284 total_size += input_count * sizeof(struct xrt_input);
285
286 // Outputs
287 size_t offset_outputs = total_size;
288 total_size += output_count * sizeof(struct xrt_output);
289
290 // HMD
291 size_t offset_hmd = total_size;
292 total_size += alloc_hmd ? sizeof(struct xrt_hmd_parts) : 0;
293
294 // Tracking
295 size_t offset_tracking = total_size;
296 total_size += alloc_tracking ? sizeof(struct xrt_tracking_origin) : 0;
297
298 // Do the allocation
299 char *ptr = U_TYPED_ARRAY_CALLOC(char, total_size);
300 struct xrt_device *xdev = (struct xrt_device *)ptr;
301
302 if (input_count > 0) {
303 xdev->input_count = input_count;
304 xdev->inputs = (struct xrt_input *)(ptr + offset_inputs);
305
306 // Set inputs to active initially, easier for drivers.
307 for (size_t i = 0; i < input_count; i++) {
308 xdev->inputs[i].active = true;
309 }
310 }
311
312 if (output_count > 0) {
313 xdev->output_count = output_count;
314 xdev->outputs = (struct xrt_output *)(ptr + offset_outputs);
315 }
316
317 if (alloc_hmd) {
318 xdev->hmd = (struct xrt_hmd_parts *)(ptr + offset_hmd);
319 // set default view count
320 xdev->hmd->view_count = 2;
321 }
322
323 if (alloc_tracking) {
324 xdev->tracking_origin = (struct xrt_tracking_origin *)(ptr + offset_tracking);
325 xdev->tracking_origin->type = XRT_TRACKING_TYPE_NONE;
326 xdev->tracking_origin->initial_offset.orientation.w = 1.0f;
327 snprintf(xdev->tracking_origin->name, XRT_TRACKING_NAME_LEN, "%s", "No tracking");
328 }
329
330 return xdev;
331}
332
333void
334u_device_free(struct xrt_device *xdev)
335{
336 if (xdev->hmd != NULL) {
337 free(xdev->hmd->distortion.mesh.vertices);
338 xdev->hmd->distortion.mesh.vertices = NULL;
339
340 free(xdev->hmd->distortion.mesh.indices);
341 xdev->hmd->distortion.mesh.indices = NULL;
342 }
343
344 free(xdev);
345}
346
347/*
348 * move the assigned xdev from hand to other_hand if:
349 * - controller of type "any hand" is assigned to hand
350 * - other_hand is unassiged
351 */
352static void
353try_move_assignment(struct xrt_device **xdevs, int *hand, int *other_hand)
354{
355 if (*hand != XRT_DEVICE_ROLE_UNASSIGNED && xdevs[*hand]->device_type == XRT_DEVICE_TYPE_ANY_HAND_CONTROLLER &&
356 *other_hand == XRT_DEVICE_ROLE_UNASSIGNED) {
357
358 *other_hand = *hand;
359 *hand = XRT_DEVICE_ROLE_UNASSIGNED;
360 }
361}
362
363void
364u_device_assign_xdev_roles(struct xrt_device **xdevs, size_t xdev_count, int *head, int *left, int *right, int *gamepad)
365{
366 *head = XRT_DEVICE_ROLE_UNASSIGNED;
367 *left = XRT_DEVICE_ROLE_UNASSIGNED;
368 *right = XRT_DEVICE_ROLE_UNASSIGNED;
369 *gamepad = XRT_DEVICE_ROLE_UNASSIGNED;
370 assert(xdev_count < INT_MAX);
371
372 for (size_t i = 0; i < xdev_count; i++) {
373 if (xdevs[i] == NULL) {
374 continue;
375 }
376
377 switch (xdevs[i]->device_type) {
378 case XRT_DEVICE_TYPE_HMD:
379 if (*head == XRT_DEVICE_ROLE_UNASSIGNED) {
380 *head = (int)i;
381 }
382 break;
383 case XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER:
384 try_move_assignment(xdevs, left, right);
385 if (*left == XRT_DEVICE_ROLE_UNASSIGNED) {
386 *left = (int)i;
387 }
388 break;
389 case XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER:
390 try_move_assignment(xdevs, right, left);
391 if (*right == XRT_DEVICE_ROLE_UNASSIGNED) {
392 *right = (int)i;
393 }
394 break;
395 case XRT_DEVICE_TYPE_GAMEPAD:
396 if (*gamepad == XRT_DEVICE_ROLE_UNASSIGNED) {
397 *gamepad = (int)i;
398 }
399 break;
400 case XRT_DEVICE_TYPE_ANY_HAND_CONTROLLER:
401 if (*left == XRT_DEVICE_ROLE_UNASSIGNED) {
402 *left = (int)i;
403 } else if (*right == XRT_DEVICE_ROLE_UNASSIGNED) {
404 *right = (int)i;
405 } else {
406 //! @todo: do something with unassigned devices?
407 }
408 break;
409 default: break;
410 }
411 }
412
413 // fill unassigned left/right with hand trackers if available
414 for (size_t i = 0; i < xdev_count; i++) {
415 if (xdevs[i] == NULL) {
416 continue;
417 }
418 if (xdevs[i]->device_type == XRT_DEVICE_TYPE_HAND_TRACKER) {
419 if (*left == XRT_DEVICE_ROLE_UNASSIGNED) {
420 *left = (int)i;
421 }
422 if (*right == XRT_DEVICE_ROLE_UNASSIGNED) {
423 *right = (int)i;
424 }
425 break;
426 }
427 }
428}
429
430void
431u_device_get_view_pose(const struct xrt_vec3 *eye_relation, uint32_t view_index, struct xrt_pose *out_pose)
432{
433 struct xrt_pose pose = XRT_POSE_IDENTITY;
434 bool adjust = view_index == 0;
435
436 pose.position.x = eye_relation->x / 2.0f;
437 pose.position.y = eye_relation->y / 2.0f;
438 pose.position.z = eye_relation->z / 2.0f;
439
440 // Adjust for left/right while also making sure there aren't any -0.f.
441 if (pose.position.x > 0.0f && adjust) {
442 pose.position.x = -pose.position.x;
443 }
444 if (pose.position.y > 0.0f && adjust) {
445 pose.position.y = -pose.position.y;
446 }
447 if (pose.position.z > 0.0f && adjust) {
448 pose.position.z = -pose.position.z;
449 }
450
451 *out_pose = pose;
452}
453
454
455/*
456 *
457 * Default implementation of functions.
458 *
459 */
460
461xrt_result_t
462u_device_get_view_poses(struct xrt_device *xdev,
463 const struct xrt_vec3 *default_eye_relation,
464 int64_t at_timestamp_ns,
465 enum xrt_view_type view_type,
466 uint32_t view_count,
467 struct xrt_space_relation *out_head_relation,
468 struct xrt_fov *out_fovs,
469 struct xrt_pose *out_poses)
470{
471 xrt_result_t xret =
472 xrt_device_get_tracked_pose(xdev, XRT_INPUT_GENERIC_HEAD_POSE, at_timestamp_ns, out_head_relation);
473 if (xret != XRT_SUCCESS) {
474 return xret;
475 }
476
477 for (uint32_t i = 0; i < view_count && i < ARRAY_SIZE(xdev->hmd->views); i++) {
478 out_fovs[i] = xdev->hmd->distortion.fov[i];
479 }
480
481 for (uint32_t i = 0; i < view_count; i++) {
482 u_device_get_view_pose(default_eye_relation, i, &out_poses[i]);
483 }
484
485 return XRT_SUCCESS;
486}
487
488xrt_result_t
489u_device_get_visibility_mask(struct xrt_device *xdev,
490 enum xrt_visibility_mask_type type,
491 uint32_t view_index,
492 struct xrt_visibility_mask **out_mask)
493{
494 const struct xrt_fov fov = xdev->hmd->distortion.fov[view_index];
495 u_visibility_mask_get_default(type, &fov, out_mask);
496 return XRT_SUCCESS;
497}
498
499/*
500 *
501 * No-op implementation of functions.
502 *
503 */
504
505xrt_result_t
506u_device_noop_update_inputs(struct xrt_device *xdev)
507{
508 // Empty, should only be used from a device without any inputs.
509 return XRT_SUCCESS;
510}
511
512
513/*
514 *
515 * Helper function to fill in defaults.
516 *
517 */
518
519void
520u_device_populate_function_pointers(struct xrt_device *xdev,
521 u_device_get_tracked_pose_function_t get_tracked_pose_fn,
522 u_device_destroy_function_t destroy_fn)
523{
524 if (get_tracked_pose_fn == NULL) {
525 U_LOG_E("Got get_tracked_pose_fn == NULL!");
526 assert(get_tracked_pose_fn != NULL);
527 }
528
529 if (destroy_fn == NULL) {
530 U_LOG_E("Got destroy_fn == NULL!");
531 assert(destroy_fn != NULL);
532 }
533
534 /*
535 * This must be implemented by the xrt_device, but not necessarily by
536 * the driver so use noop version.
537 */
538 xdev->update_inputs = u_device_noop_update_inputs;
539
540 // This must be implemented by the driver.
541 xdev->get_tracked_pose = get_tracked_pose_fn;
542
543 /*
544 * These are not required to be implemented by the xrt_device, so use
545 * not implemented versions, and let the driver override if needed.
546 */
547 xdev->get_hand_tracking = u_device_ni_get_hand_tracking;
548 xdev->get_face_tracking = u_device_ni_get_face_tracking;
549 xdev->get_body_skeleton = u_device_ni_get_body_skeleton;
550 xdev->get_body_joints = u_device_ni_get_body_joints;
551 xdev->reset_body_tracking_calibration_meta = u_device_ni_reset_body_tracking_calibration_meta;
552 xdev->set_body_tracking_calibration_override_meta = u_device_ni_set_body_tracking_calibration_override_meta;
553 xdev->set_output = u_device_ni_set_output;
554 xdev->get_output_limits = u_device_ni_get_output_limits;
555 xdev->get_presence = u_device_ni_get_presence;
556 xdev->begin_plane_detection_ext = u_device_ni_begin_plane_detection_ext;
557 xdev->destroy_plane_detection_ext = u_device_ni_destroy_plane_detection_ext;
558 xdev->get_plane_detection_state_ext = u_device_ni_get_plane_detection_state_ext;
559 xdev->get_plane_detections_ext = u_device_ni_get_plane_detections_ext;
560 xdev->get_view_poses = u_device_ni_get_view_poses;
561 xdev->compute_distortion = u_device_ni_compute_distortion;
562 xdev->get_visibility_mask = u_device_ni_get_visibility_mask;
563 xdev->ref_space_usage = u_device_ni_ref_space_usage;
564 xdev->is_form_factor_available = u_device_ni_is_form_factor_available;
565 xdev->get_battery_status = u_device_ni_get_battery_status;
566 xdev->get_brightness = u_device_ni_get_brightness;
567 xdev->set_brightness = u_device_ni_set_brightness;
568 xdev->begin_feature = u_device_ni_begin_feature;
569 xdev->end_feature = u_device_ni_end_feature;
570
571 // This must be implemented by the driver.
572 xdev->destroy = destroy_fn;
573}