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 controller 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_var.h"
18#include "util/u_misc.h"
19#include "util/u_time.h"
20#include "util/u_debug.h"
21#include "util/u_device.h"
22#include "util/u_logging.h"
23#include "util/u_distortion_mesh.h"
24
25#include "simulated_interface.h"
26
27#include <stdio.h>
28#include <assert.h>
29
30
31/*
32 *
33 * Structs and defines.
34 *
35 */
36
37struct simulated_device
38{
39 struct xrt_device base;
40
41 struct xrt_pose center;
42
43 bool active;
44};
45
46#define CHECK_THAT_NAME_IS_AND_ERROR(NAME) \
47 do { \
48 if (sd->base.name != NAME) { \
49 U_LOG_XDEV_UNSUPPORTED_INPUT(&sd->base, u_log_get_global_level(), name); \
50 return XRT_ERROR_INPUT_UNSUPPORTED; \
51 } \
52 } while (false)
53
54
55/*
56 *
57 * Helper functions.
58 *
59 */
60
61static inline struct simulated_device *
62simulated_device(struct xrt_device *xdev)
63{
64 return (struct simulated_device *)xdev;
65}
66
67static const char *
68device_type_to_printable_handedness(enum xrt_device_type type)
69{
70 switch (type) {
71 case XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER: return " Left";
72 case XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER: return " Right";
73 default: assert(false && "Must be valid handedness"); return NULL;
74 }
75}
76
77
78/*
79 *
80 * Member functions.
81 *
82 */
83
84static void
85simulated_device_destroy(struct xrt_device *xdev)
86{
87 struct simulated_device *sd = simulated_device(xdev);
88
89 // Remove the variable tracking.
90 u_var_remove_root(sd);
91
92 // Free this device with the helper.
93 u_device_free(&sd->base);
94}
95
96static xrt_result_t
97simulated_device_update_inputs(struct xrt_device *xdev)
98{
99 struct simulated_device *sd = simulated_device(xdev);
100
101 uint64_t now = os_monotonic_get_ns();
102
103 // TODO refactor those loops into one
104 if (!sd->active) {
105 for (uint32_t i = 0; i < xdev->input_count; i++) {
106 xdev->inputs[i].active = false;
107 xdev->inputs[i].timestamp = now;
108 U_ZERO(&xdev->inputs[i].value);
109 }
110 return XRT_SUCCESS;
111 }
112
113 for (uint32_t i = 0; i < xdev->input_count; i++) {
114 xdev->inputs[i].active = true;
115 xdev->inputs[i].timestamp = now;
116 }
117
118 return XRT_SUCCESS;
119}
120
121static xrt_result_t
122simulated_device_get_tracked_pose(struct xrt_device *xdev,
123 enum xrt_input_name name,
124 int64_t at_timestamp_ns,
125 struct xrt_space_relation *out_relation)
126{
127 struct simulated_device *sd = simulated_device(xdev);
128
129 switch (name) {
130 case XRT_INPUT_SIMPLE_GRIP_POSE:
131 case XRT_INPUT_SIMPLE_AIM_POSE: CHECK_THAT_NAME_IS_AND_ERROR(XRT_DEVICE_SIMPLE_CONTROLLER); break;
132 case XRT_INPUT_WMR_GRIP_POSE:
133 case XRT_INPUT_WMR_AIM_POSE: CHECK_THAT_NAME_IS_AND_ERROR(XRT_DEVICE_WMR_CONTROLLER); break;
134 case XRT_INPUT_ML2_CONTROLLER_GRIP_POSE:
135 case XRT_INPUT_ML2_CONTROLLER_AIM_POSE: CHECK_THAT_NAME_IS_AND_ERROR(XRT_DEVICE_ML2_CONTROLLER); break;
136 default:
137 U_LOG_XDEV_UNSUPPORTED_INPUT(&sd->base, u_log_get_global_level(), name);
138 return XRT_ERROR_INPUT_UNSUPPORTED;
139 }
140
141 if (!sd->active) {
142 out_relation->pose = (struct xrt_pose)XRT_POSE_IDENTITY;
143 out_relation->relation_flags = 0;
144 return XRT_SUCCESS;
145 }
146
147 struct xrt_pose pose = sd->center;
148 struct xrt_vec3 linear_velocity = XRT_VEC3_ZERO;
149 struct xrt_vec3 angular_velocity = XRT_VEC3_ZERO;
150
151 /*
152 * It's easier to reason about angular velocity if it's controlled in
153 * body space, but the angular velocity returned in the relation is in
154 * the base space.
155 */
156 math_quat_rotate_derivative(&pose.orientation, &angular_velocity, &out_relation->angular_velocity);
157
158 out_relation->pose = pose;
159 out_relation->linear_velocity = linear_velocity;
160
161 out_relation->relation_flags = (enum xrt_space_relation_flags)(
162 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT |
163 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT |
164 XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
165
166 return XRT_SUCCESS;
167}
168
169static void
170simulated_device_get_hand_tracking(struct xrt_device *xdev,
171 enum xrt_input_name name,
172 int64_t requested_timestamp_ns,
173 struct xrt_hand_joint_set *out_value,
174 int64_t *out_timestamp_ns)
175{
176 assert(false);
177}
178
179static void
180simulated_device_get_view_poses(struct xrt_device *xdev,
181 const struct xrt_vec3 *default_eye_relation,
182 int64_t at_timestamp_ns,
183 uint32_t view_count,
184 struct xrt_space_relation *out_head_relation,
185 struct xrt_fov *out_fovs,
186 struct xrt_pose *out_poses)
187{
188 assert(false);
189}
190
191static void
192simulated_device_set_output(struct xrt_device *xdev, enum xrt_output_name name, const union xrt_output_value *value)
193{
194 struct simulated_device *sd = simulated_device(xdev);
195 (void)sd;
196}
197
198
199/*
200 *
201 * Various data driven arrays.
202 *
203 */
204
205/*
206 * Simple Controller.
207 */
208
209static enum xrt_input_name simple_inputs_array[] = {
210 XRT_INPUT_SIMPLE_SELECT_CLICK,
211 XRT_INPUT_SIMPLE_MENU_CLICK,
212 XRT_INPUT_SIMPLE_GRIP_POSE,
213 XRT_INPUT_SIMPLE_AIM_POSE,
214};
215
216static enum xrt_output_name simple_outputs_array[] = {
217 XRT_OUTPUT_NAME_SIMPLE_VIBRATION,
218};
219
220
221/*
222 * WinMR Controller.
223 */
224
225static enum xrt_input_name wmr_inputs_array[] = {
226 XRT_INPUT_WMR_MENU_CLICK, XRT_INPUT_WMR_SQUEEZE_CLICK, XRT_INPUT_WMR_TRIGGER_VALUE,
227 XRT_INPUT_WMR_THUMBSTICK_CLICK, XRT_INPUT_WMR_THUMBSTICK, XRT_INPUT_WMR_TRACKPAD_CLICK,
228 XRT_INPUT_WMR_TRACKPAD_TOUCH, XRT_INPUT_WMR_TRACKPAD, XRT_INPUT_WMR_GRIP_POSE,
229 XRT_INPUT_WMR_AIM_POSE,
230};
231
232static enum xrt_output_name wmr_outputs_array[] = {
233 XRT_OUTPUT_NAME_WMR_HAPTIC,
234};
235
236static struct xrt_binding_input_pair wmr_to_simple_inputs[4] = {
237 {XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_WMR_TRIGGER_VALUE},
238 {XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_WMR_MENU_CLICK},
239 {XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_WMR_GRIP_POSE},
240 {XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_WMR_AIM_POSE},
241};
242
243static struct xrt_binding_output_pair wmr_to_simple_outputs[1] = {
244 {XRT_OUTPUT_NAME_SIMPLE_VIBRATION, XRT_OUTPUT_NAME_INDEX_HAPTIC},
245};
246
247static struct xrt_binding_profile wmr_binding_profiles[1] = {
248 {
249 .name = XRT_DEVICE_SIMPLE_CONTROLLER,
250 .inputs = wmr_to_simple_inputs,
251 .input_count = ARRAY_SIZE(wmr_to_simple_inputs),
252 .outputs = wmr_to_simple_outputs,
253 .output_count = ARRAY_SIZE(wmr_to_simple_outputs),
254 },
255};
256
257
258/*
259 * ML2 Controller.
260 */
261
262static enum xrt_input_name ml2_inputs_array[] = {
263 XRT_INPUT_ML2_CONTROLLER_MENU_CLICK, XRT_INPUT_ML2_CONTROLLER_SELECT_CLICK,
264 XRT_INPUT_ML2_CONTROLLER_TRIGGER_CLICK, XRT_INPUT_ML2_CONTROLLER_TRIGGER_VALUE,
265 XRT_INPUT_ML2_CONTROLLER_TRACKPAD_CLICK, XRT_INPUT_ML2_CONTROLLER_TRACKPAD_TOUCH,
266 XRT_INPUT_ML2_CONTROLLER_TRACKPAD_FORCE, XRT_INPUT_ML2_CONTROLLER_TRACKPAD,
267 XRT_INPUT_ML2_CONTROLLER_GRIP_POSE, XRT_INPUT_ML2_CONTROLLER_AIM_POSE,
268 XRT_INPUT_ML2_CONTROLLER_SHOULDER_CLICK,
269};
270
271static enum xrt_output_name ml2_outputs_array[] = {
272 XRT_OUTPUT_NAME_ML2_CONTROLLER_VIBRATION,
273};
274
275static struct xrt_binding_input_pair ml2_to_simple_inputs[4] = {
276 {XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_ML2_CONTROLLER_TRIGGER_VALUE},
277 {XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_ML2_CONTROLLER_MENU_CLICK},
278 {XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_ML2_CONTROLLER_GRIP_POSE},
279 {XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_ML2_CONTROLLER_AIM_POSE},
280};
281
282static struct xrt_binding_output_pair ml2_to_simple_outputs[1] = {
283 {XRT_OUTPUT_NAME_SIMPLE_VIBRATION, XRT_OUTPUT_NAME_INDEX_HAPTIC},
284};
285
286static struct xrt_binding_input_pair ml2_to_vive_wand_inputs[9] = {
287 {XRT_INPUT_VIVE_GRIP_POSE, XRT_INPUT_ML2_CONTROLLER_GRIP_POSE},
288 {XRT_INPUT_VIVE_AIM_POSE, XRT_INPUT_ML2_CONTROLLER_AIM_POSE},
289 {XRT_INPUT_VIVE_TRIGGER_CLICK, XRT_INPUT_ML2_CONTROLLER_TRIGGER_CLICK},
290 {XRT_INPUT_VIVE_TRIGGER_VALUE, XRT_INPUT_ML2_CONTROLLER_TRIGGER_VALUE},
291 {XRT_INPUT_VIVE_SQUEEZE_CLICK, XRT_INPUT_ML2_CONTROLLER_SHOULDER_CLICK},
292 // {XRT_INPUT_VIVE_SYSTEM_CLICK, NONE},
293 {XRT_INPUT_VIVE_MENU_CLICK, XRT_INPUT_ML2_CONTROLLER_MENU_CLICK},
294 {XRT_INPUT_VIVE_TRACKPAD, XRT_INPUT_ML2_CONTROLLER_TRACKPAD},
295 // {NONE, XRT_INPUT_ML2_CONTROLLER_TRACKPAD_FORCE},
296 {XRT_INPUT_VIVE_TRACKPAD_TOUCH, XRT_INPUT_ML2_CONTROLLER_TRACKPAD_TOUCH},
297 {XRT_INPUT_VIVE_TRACKPAD_CLICK, XRT_INPUT_ML2_CONTROLLER_TRACKPAD_CLICK},
298};
299
300static struct xrt_binding_output_pair ml2_to_vive_wand_outputs[1] = {
301 {XRT_OUTPUT_NAME_VIVE_HAPTIC, XRT_OUTPUT_NAME_ML2_CONTROLLER_VIBRATION},
302};
303
304static struct xrt_binding_profile ml2_binding_profiles[2] = {
305 {
306 .name = XRT_DEVICE_SIMPLE_CONTROLLER,
307 .inputs = ml2_to_simple_inputs,
308 .input_count = ARRAY_SIZE(ml2_to_simple_inputs),
309 .outputs = ml2_to_simple_outputs,
310 .output_count = ARRAY_SIZE(ml2_to_simple_outputs),
311 },
312 {
313 .name = XRT_DEVICE_VIVE_WAND,
314 .inputs = ml2_to_vive_wand_inputs,
315 .input_count = ARRAY_SIZE(ml2_to_vive_wand_inputs),
316 .outputs = ml2_to_vive_wand_outputs,
317 .output_count = ARRAY_SIZE(ml2_to_vive_wand_outputs),
318 },
319};
320
321
322/*
323 *
324 * 'Exported' functions.
325 *
326 */
327
328struct xrt_device *
329simulated_create_controller(enum xrt_device_name name,
330 enum xrt_device_type type,
331 const struct xrt_pose *center,
332 struct xrt_tracking_origin *origin)
333{
334 const enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE;
335 const char *handedness = "";
336 const char *name_str = NULL;
337 enum xrt_input_name *inputs = NULL;
338 uint32_t input_count = 0;
339 enum xrt_output_name *outputs = NULL;
340 uint32_t output_count = 0;
341 struct xrt_binding_profile *binding_profiles = NULL;
342 uint32_t binding_profile_count = 0;
343
344 switch (name) {
345 case XRT_DEVICE_SIMPLE_CONTROLLER:
346 name_str = "Simple";
347 input_count = ARRAY_SIZE(simple_inputs_array);
348 output_count = ARRAY_SIZE(simple_outputs_array);
349 inputs = simple_inputs_array;
350 outputs = simple_outputs_array;
351 assert(type == XRT_DEVICE_TYPE_ANY_HAND_CONTROLLER);
352 break;
353 case XRT_DEVICE_WMR_CONTROLLER:
354 name_str = "WinMR";
355 input_count = ARRAY_SIZE(wmr_inputs_array);
356 output_count = ARRAY_SIZE(wmr_outputs_array);
357 inputs = wmr_inputs_array;
358 outputs = wmr_outputs_array;
359 binding_profiles = wmr_binding_profiles;
360 binding_profile_count = ARRAY_SIZE(wmr_binding_profiles);
361 handedness = device_type_to_printable_handedness(type);
362 break;
363 case XRT_DEVICE_ML2_CONTROLLER:
364 name_str = "ML2";
365 input_count = ARRAY_SIZE(ml2_inputs_array);
366 output_count = ARRAY_SIZE(ml2_outputs_array);
367 inputs = ml2_inputs_array;
368 outputs = ml2_outputs_array;
369 binding_profiles = ml2_binding_profiles;
370 binding_profile_count = ARRAY_SIZE(ml2_binding_profiles);
371
372 assert(type == XRT_DEVICE_TYPE_ANY_HAND_CONTROLLER);
373 break;
374 default: assert(false); return NULL;
375 }
376
377 // Allocate.
378 struct simulated_device *sd = U_DEVICE_ALLOCATE(struct simulated_device, flags, input_count, output_count);
379 sd->base.update_inputs = simulated_device_update_inputs;
380 sd->base.get_tracked_pose = simulated_device_get_tracked_pose;
381 sd->base.get_hand_tracking = simulated_device_get_hand_tracking;
382 sd->base.get_view_poses = simulated_device_get_view_poses;
383 sd->base.set_output = simulated_device_set_output;
384 sd->base.destroy = simulated_device_destroy;
385 sd->base.tracking_origin = origin;
386 sd->base.orientation_tracking_supported = true;
387 sd->base.position_tracking_supported = true;
388 sd->base.hand_tracking_supported = false;
389 sd->base.name = name;
390 sd->base.device_type = type;
391 sd->base.binding_profiles = binding_profiles;
392 sd->base.binding_profile_count = binding_profile_count;
393
394 snprintf(sd->base.str, sizeof(sd->base.str), "%s%s Controller (Simulated)", name_str, handedness);
395 snprintf(sd->base.serial, sizeof(sd->base.str), "%s%s Controller (Simulated)", name_str, handedness);
396
397 for (uint32_t i = 0; i < input_count; i++) {
398 sd->base.inputs[i].active = true;
399 sd->base.inputs[i].name = inputs[i];
400 }
401
402 for (uint32_t i = 0; i < output_count; i++) {
403 sd->base.outputs[i].name = outputs[i];
404 }
405
406 sd->center = *center;
407 sd->active = true;
408
409 u_var_add_root(sd, sd->base.str, true);
410 u_var_add_pose(sd, &sd->center, "center");
411 u_var_add_bool(sd, &sd->active, "active");
412
413 return &sd->base;
414}