matrixfurry.com
The open source OpenXR runtime
1// Copyright 2020-2023, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Simulated HMD device.
6 * @author Jakob Bornecrantz <jakob@collabora.com>
7 * @ingroup drv_simulated
8 */
9
10#include "xrt/xrt_device.h"
11
12#include "os/os_time.h"
13
14#include "math/m_api.h"
15#include "math/m_mathinclude.h"
16
17#include "util/u_debug.h"
18#include "util/u_device.h"
19#include "util/u_distortion_mesh.h"
20#include "util/u_logging.h"
21#include "util/u_misc.h"
22#include "util/u_pretty_print.h"
23#include "util/u_time.h"
24#include "util/u_var.h"
25
26#include "simulated_interface.h"
27
28#include <stdio.h>
29
30
31/*
32 *
33 * Structs and defines.
34 *
35 */
36
37/*!
38 * A example HMD device.
39 *
40 * @implements xrt_device
41 */
42struct simulated_hmd
43{
44 struct xrt_device base;
45
46 struct xrt_pose pose;
47 struct xrt_pose center;
48
49 uint64_t created_ns;
50 float diameter_m;
51
52 enum u_logging_level log_level;
53 enum simulated_movement movement;
54};
55
56
57/*
58 *
59 * Functions
60 *
61 */
62
63static inline struct simulated_hmd *
64simulated_hmd(struct xrt_device *xdev)
65{
66 return (struct simulated_hmd *)xdev;
67}
68
69DEBUG_GET_ONCE_LOG_OPTION(simulated_log, "SIMULATED_LOG", U_LOGGING_WARN)
70DEBUG_GET_ONCE_NUM_OPTION(view_count, "SIMULATED_VIEW_COUNT", 2)
71
72#define HMD_TRACE(hmd, ...) U_LOG_XDEV_IFL_T(&hmd->base, hmd->log_level, __VA_ARGS__)
73#define HMD_DEBUG(hmd, ...) U_LOG_XDEV_IFL_D(&hmd->base, hmd->log_level, __VA_ARGS__)
74#define HMD_INFO(hmd, ...) U_LOG_XDEV_IFL_I(&hmd->base, hmd->log_level, __VA_ARGS__)
75#define HMD_ERROR(hmd, ...) U_LOG_XDEV_IFL_E(&hmd->base, hmd->log_level, __VA_ARGS__)
76
77static void
78simulated_hmd_destroy(struct xrt_device *xdev)
79{
80 struct simulated_hmd *dh = simulated_hmd(xdev);
81
82 // Remove the variable tracking.
83 u_var_remove_root(dh);
84
85 u_device_free(&dh->base);
86}
87
88static xrt_result_t
89simulated_hmd_get_tracked_pose(struct xrt_device *xdev,
90 enum xrt_input_name name,
91 int64_t at_timestamp_ns,
92 struct xrt_space_relation *out_relation)
93{
94 struct simulated_hmd *hmd = simulated_hmd(xdev);
95
96 if (name != XRT_INPUT_GENERIC_HEAD_POSE) {
97 U_LOG_XDEV_UNSUPPORTED_INPUT(&hmd->base, hmd->log_level, name);
98 return XRT_ERROR_INPUT_UNSUPPORTED;
99 }
100
101 const double time_s = time_ns_to_s(at_timestamp_ns - hmd->created_ns);
102 const double d = hmd->diameter_m;
103 const double d2 = d * 2;
104 const double t = 2.0;
105 const double t2 = t * 2;
106 const double t3 = t * 3;
107 const double t4 = t * 4;
108 const struct xrt_vec3 up = {0, 1, 0};
109
110 switch (hmd->movement) {
111 default:
112 case SIMULATED_MOVEMENT_WOBBLE: {
113 struct xrt_pose tmp = XRT_POSE_IDENTITY;
114
115 // Wobble time.
116 tmp.position.x = sin((time_s / t2) * M_PI) * d2 - d;
117 tmp.position.y = sin((time_s / t) * M_PI) * d;
118 tmp.orientation.x = sin((time_s / t3) * M_PI) / 64.0f;
119 tmp.orientation.y = sin((time_s / t4) * M_PI) / 16.0f;
120 tmp.orientation.z = sin((time_s / t4) * M_PI) / 64.0f;
121 math_quat_normalize(&tmp.orientation);
122
123 // Transform with center to set it.
124 math_pose_transform(&hmd->center, &tmp, &hmd->pose);
125 } break;
126 case SIMULATED_MOVEMENT_ROTATE: {
127 struct xrt_pose tmp = XRT_POSE_IDENTITY;
128
129 // Rotate around the up vector.
130 math_quat_from_angle_vector(time_s / 4, &up, &hmd->pose.orientation);
131
132 // Transform with center to set it.
133 math_pose_transform(&hmd->center, &tmp, &hmd->pose);
134 } break;
135 case SIMULATED_MOVEMENT_STATIONARY:
136 // Reset pose.
137 hmd->pose = hmd->center;
138 break;
139 }
140
141 out_relation->pose = hmd->pose;
142 out_relation->relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
143 XRT_SPACE_RELATION_POSITION_VALID_BIT |
144 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
145
146 return XRT_SUCCESS;
147}
148
149static xrt_result_t
150simulated_ref_space_usage(struct xrt_device *xdev,
151 enum xrt_reference_space_type type,
152 enum xrt_input_name name,
153 bool used)
154{
155 struct simulated_hmd *hmd = simulated_hmd(xdev);
156
157 struct u_pp_sink_stack_only sink;
158 u_pp_delegate_t dg = u_pp_sink_stack_only_init(&sink);
159
160 u_pp(dg, "Ref space ");
161 u_pp_xrt_reference_space_type(dg, type);
162 u_pp(dg, " is %sused", used ? "" : "not ");
163
164 if (name != 0) {
165 u_pp(dg, ", driven by ");
166 u_pp_xrt_input_name(dg, name);
167 u_pp(dg, ".");
168 } else {
169 u_pp(dg, ", not controlled by us.");
170 }
171
172 HMD_INFO(hmd, "%s", sink.buffer);
173
174 return XRT_SUCCESS;
175}
176
177/*
178 *
179 * 'Exported' functions.
180 *
181 */
182
183enum u_logging_level
184simulated_log_level(void)
185{
186 return debug_get_log_option_simulated_log();
187}
188
189struct xrt_device *
190simulated_hmd_create(enum simulated_movement movement, const struct xrt_pose *center)
191{
192 enum u_device_alloc_flags flags =
193 (enum u_device_alloc_flags)(U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE);
194 struct simulated_hmd *hmd = U_DEVICE_ALLOCATE(struct simulated_hmd, flags, 1, 0);
195 hmd->base.update_inputs = u_device_noop_update_inputs;
196 hmd->base.get_tracked_pose = simulated_hmd_get_tracked_pose;
197 hmd->base.get_view_poses = u_device_get_view_poses;
198 hmd->base.get_visibility_mask = u_device_get_visibility_mask;
199 hmd->base.ref_space_usage = simulated_ref_space_usage;
200 hmd->base.destroy = simulated_hmd_destroy;
201 hmd->base.name = XRT_DEVICE_GENERIC_HMD;
202 hmd->base.device_type = XRT_DEVICE_TYPE_HMD;
203 hmd->base.supported.ref_space_usage = true;
204 hmd->pose.orientation.w = 1.0f; // All other values set to zero.
205 hmd->center = *center;
206 hmd->created_ns = os_monotonic_get_ns();
207 hmd->diameter_m = 0.05f;
208 hmd->log_level = simulated_log_level();
209 hmd->movement = movement;
210
211 hmd->base.hmd->view_count = debug_get_num_option_view_count();
212 // Print name.
213 snprintf(hmd->base.str, XRT_DEVICE_NAME_LEN, "Simulated HMD");
214 snprintf(hmd->base.serial, XRT_DEVICE_NAME_LEN, "Simulated HMD");
215
216 // Setup input.
217 hmd->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE;
218
219 // Setup info.
220 bool ret = true;
221 struct u_device_simple_info info;
222 info.display.w_pixels = 1280;
223 info.display.h_pixels = 720;
224 info.display.w_meters = 0.13f;
225 info.display.h_meters = 0.07f;
226 info.lens_horizontal_separation_meters = 0.13f / 2.0f;
227 info.lens_vertical_position_meters = 0.07f / 2.0f;
228
229 if (hmd->base.hmd->view_count == 1) {
230 info.fov[0] = 120.0f * (M_PI / 180.0f);
231 ret = u_device_setup_one_eye(&hmd->base, &info);
232 } else if (hmd->base.hmd->view_count == 2) {
233 info.fov[0] = 85.0f * (M_PI / 180.0f);
234 info.fov[1] = 85.0f * (M_PI / 180.0f);
235 ret = u_device_setup_split_side_by_side(&hmd->base, &info);
236 } else {
237 U_LOG_E("Invalid view count");
238 ret = false;
239 }
240 if (!ret) {
241 HMD_ERROR(hmd, "Failed to setup basic device info");
242 simulated_hmd_destroy(&hmd->base);
243 return NULL;
244 }
245
246 // Setup variable tracker.
247 u_var_add_root(hmd, "Simulated HMD", true);
248 u_var_add_pose(hmd, &hmd->pose, "pose");
249 u_var_add_pose(hmd, &hmd->center, "center");
250 u_var_add_f32(hmd, &hmd->diameter_m, "diameter_m");
251 u_var_add_log_level(hmd, &hmd->log_level, "log_level");
252
253 // Distortion information, fills in xdev->compute_distortion().
254 u_distortion_mesh_set_none(&hmd->base);
255
256 return &hmd->base;
257}