The open source OpenXR runtime
matrixfurry.com
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
169
170/*
171 *
172 * Various data driven arrays.
173 *
174 */
175
176/*
177 * Simple Controller.
178 */
179
180static enum xrt_input_name simple_inputs_array[] = {
181 XRT_INPUT_SIMPLE_SELECT_CLICK,
182 XRT_INPUT_SIMPLE_MENU_CLICK,
183 XRT_INPUT_SIMPLE_GRIP_POSE,
184 XRT_INPUT_SIMPLE_AIM_POSE,
185};
186
187static enum xrt_output_name simple_outputs_array[] = {
188 XRT_OUTPUT_NAME_SIMPLE_VIBRATION,
189};
190
191
192/*
193 * WinMR Controller.
194 */
195
196static enum xrt_input_name wmr_inputs_array[] = {
197 XRT_INPUT_WMR_MENU_CLICK, XRT_INPUT_WMR_SQUEEZE_CLICK, XRT_INPUT_WMR_TRIGGER_VALUE,
198 XRT_INPUT_WMR_THUMBSTICK_CLICK, XRT_INPUT_WMR_THUMBSTICK, XRT_INPUT_WMR_TRACKPAD_CLICK,
199 XRT_INPUT_WMR_TRACKPAD_TOUCH, XRT_INPUT_WMR_TRACKPAD, XRT_INPUT_WMR_GRIP_POSE,
200 XRT_INPUT_WMR_AIM_POSE,
201};
202
203static enum xrt_output_name wmr_outputs_array[] = {
204 XRT_OUTPUT_NAME_WMR_HAPTIC,
205};
206
207static struct xrt_binding_input_pair wmr_to_simple_inputs[4] = {
208 {XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_WMR_TRIGGER_VALUE},
209 {XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_WMR_MENU_CLICK},
210 {XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_WMR_GRIP_POSE},
211 {XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_WMR_AIM_POSE},
212};
213
214static struct xrt_binding_output_pair wmr_to_simple_outputs[1] = {
215 {XRT_OUTPUT_NAME_SIMPLE_VIBRATION, XRT_OUTPUT_NAME_INDEX_HAPTIC},
216};
217
218static struct xrt_binding_profile wmr_binding_profiles[1] = {
219 {
220 .name = XRT_DEVICE_SIMPLE_CONTROLLER,
221 .inputs = wmr_to_simple_inputs,
222 .input_count = ARRAY_SIZE(wmr_to_simple_inputs),
223 .outputs = wmr_to_simple_outputs,
224 .output_count = ARRAY_SIZE(wmr_to_simple_outputs),
225 },
226};
227
228
229/*
230 * ML2 Controller.
231 */
232
233static enum xrt_input_name ml2_inputs_array[] = {
234 XRT_INPUT_ML2_CONTROLLER_MENU_CLICK, XRT_INPUT_ML2_CONTROLLER_SELECT_CLICK,
235 XRT_INPUT_ML2_CONTROLLER_TRIGGER_CLICK, XRT_INPUT_ML2_CONTROLLER_TRIGGER_VALUE,
236 XRT_INPUT_ML2_CONTROLLER_TRACKPAD_CLICK, XRT_INPUT_ML2_CONTROLLER_TRACKPAD_TOUCH,
237 XRT_INPUT_ML2_CONTROLLER_TRACKPAD_FORCE, XRT_INPUT_ML2_CONTROLLER_TRACKPAD,
238 XRT_INPUT_ML2_CONTROLLER_GRIP_POSE, XRT_INPUT_ML2_CONTROLLER_AIM_POSE,
239 XRT_INPUT_ML2_CONTROLLER_SHOULDER_CLICK,
240};
241
242static enum xrt_output_name ml2_outputs_array[] = {
243 XRT_OUTPUT_NAME_ML2_CONTROLLER_VIBRATION,
244};
245
246static struct xrt_binding_input_pair ml2_to_simple_inputs[4] = {
247 {XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_ML2_CONTROLLER_TRIGGER_VALUE},
248 {XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_ML2_CONTROLLER_MENU_CLICK},
249 {XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_ML2_CONTROLLER_GRIP_POSE},
250 {XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_ML2_CONTROLLER_AIM_POSE},
251};
252
253static struct xrt_binding_output_pair ml2_to_simple_outputs[1] = {
254 {XRT_OUTPUT_NAME_SIMPLE_VIBRATION, XRT_OUTPUT_NAME_INDEX_HAPTIC},
255};
256
257static struct xrt_binding_input_pair ml2_to_vive_wand_inputs[9] = {
258 {XRT_INPUT_VIVE_GRIP_POSE, XRT_INPUT_ML2_CONTROLLER_GRIP_POSE},
259 {XRT_INPUT_VIVE_AIM_POSE, XRT_INPUT_ML2_CONTROLLER_AIM_POSE},
260 {XRT_INPUT_VIVE_TRIGGER_CLICK, XRT_INPUT_ML2_CONTROLLER_TRIGGER_CLICK},
261 {XRT_INPUT_VIVE_TRIGGER_VALUE, XRT_INPUT_ML2_CONTROLLER_TRIGGER_VALUE},
262 {XRT_INPUT_VIVE_SQUEEZE_CLICK, XRT_INPUT_ML2_CONTROLLER_SHOULDER_CLICK},
263 // {XRT_INPUT_VIVE_SYSTEM_CLICK, NONE},
264 {XRT_INPUT_VIVE_MENU_CLICK, XRT_INPUT_ML2_CONTROLLER_MENU_CLICK},
265 {XRT_INPUT_VIVE_TRACKPAD, XRT_INPUT_ML2_CONTROLLER_TRACKPAD},
266 // {NONE, XRT_INPUT_ML2_CONTROLLER_TRACKPAD_FORCE},
267 {XRT_INPUT_VIVE_TRACKPAD_TOUCH, XRT_INPUT_ML2_CONTROLLER_TRACKPAD_TOUCH},
268 {XRT_INPUT_VIVE_TRACKPAD_CLICK, XRT_INPUT_ML2_CONTROLLER_TRACKPAD_CLICK},
269};
270
271static struct xrt_binding_output_pair ml2_to_vive_wand_outputs[1] = {
272 {XRT_OUTPUT_NAME_VIVE_HAPTIC, XRT_OUTPUT_NAME_ML2_CONTROLLER_VIBRATION},
273};
274
275static struct xrt_binding_profile ml2_binding_profiles[2] = {
276 {
277 .name = XRT_DEVICE_SIMPLE_CONTROLLER,
278 .inputs = ml2_to_simple_inputs,
279 .input_count = ARRAY_SIZE(ml2_to_simple_inputs),
280 .outputs = ml2_to_simple_outputs,
281 .output_count = ARRAY_SIZE(ml2_to_simple_outputs),
282 },
283 {
284 .name = XRT_DEVICE_VIVE_WAND,
285 .inputs = ml2_to_vive_wand_inputs,
286 .input_count = ARRAY_SIZE(ml2_to_vive_wand_inputs),
287 .outputs = ml2_to_vive_wand_outputs,
288 .output_count = ARRAY_SIZE(ml2_to_vive_wand_outputs),
289 },
290};
291
292
293/*
294 *
295 * 'Exported' functions.
296 *
297 */
298
299struct xrt_device *
300simulated_create_controller(enum xrt_device_name name,
301 enum xrt_device_type type,
302 const struct xrt_pose *center,
303 struct xrt_tracking_origin *origin)
304{
305 const enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE;
306 const char *handedness = "";
307 const char *name_str = NULL;
308 enum xrt_input_name *inputs = NULL;
309 uint32_t input_count = 0;
310 enum xrt_output_name *outputs = NULL;
311 uint32_t output_count = 0;
312 struct xrt_binding_profile *binding_profiles = NULL;
313 uint32_t binding_profile_count = 0;
314
315 switch (name) {
316 case XRT_DEVICE_SIMPLE_CONTROLLER:
317 name_str = "Simple";
318 input_count = ARRAY_SIZE(simple_inputs_array);
319 output_count = ARRAY_SIZE(simple_outputs_array);
320 inputs = simple_inputs_array;
321 outputs = simple_outputs_array;
322 assert(type == XRT_DEVICE_TYPE_ANY_HAND_CONTROLLER);
323 break;
324 case XRT_DEVICE_WMR_CONTROLLER:
325 name_str = "WinMR";
326 input_count = ARRAY_SIZE(wmr_inputs_array);
327 output_count = ARRAY_SIZE(wmr_outputs_array);
328 inputs = wmr_inputs_array;
329 outputs = wmr_outputs_array;
330 binding_profiles = wmr_binding_profiles;
331 binding_profile_count = ARRAY_SIZE(wmr_binding_profiles);
332 handedness = device_type_to_printable_handedness(type);
333 break;
334 case XRT_DEVICE_ML2_CONTROLLER:
335 name_str = "ML2";
336 input_count = ARRAY_SIZE(ml2_inputs_array);
337 output_count = ARRAY_SIZE(ml2_outputs_array);
338 inputs = ml2_inputs_array;
339 outputs = ml2_outputs_array;
340 binding_profiles = ml2_binding_profiles;
341 binding_profile_count = ARRAY_SIZE(ml2_binding_profiles);
342
343 assert(type == XRT_DEVICE_TYPE_ANY_HAND_CONTROLLER);
344 break;
345 default: assert(false); return NULL;
346 }
347
348 // Allocate.
349 struct simulated_device *sd = U_DEVICE_ALLOCATE(struct simulated_device, flags, input_count, output_count);
350 u_device_populate_function_pointers(&sd->base, simulated_device_get_tracked_pose, simulated_device_destroy);
351 sd->base.update_inputs = simulated_device_update_inputs;
352 sd->base.tracking_origin = origin;
353 sd->base.supported.orientation_tracking = true;
354 sd->base.supported.position_tracking = true;
355 sd->base.supported.hand_tracking = false;
356 sd->base.name = name;
357 sd->base.device_type = type;
358 sd->base.binding_profiles = binding_profiles;
359 sd->base.binding_profile_count = binding_profile_count;
360
361 snprintf(sd->base.str, sizeof(sd->base.str), "%s%s Controller (Simulated)", name_str, handedness);
362 snprintf(sd->base.serial, sizeof(sd->base.str), "%s%s Controller (Simulated)", name_str, handedness);
363
364 for (uint32_t i = 0; i < input_count; i++) {
365 sd->base.inputs[i].active = true;
366 sd->base.inputs[i].name = inputs[i];
367 }
368
369 for (uint32_t i = 0; i < output_count; i++) {
370 sd->base.outputs[i].name = outputs[i];
371 }
372
373 sd->center = *center;
374 sd->active = true;
375
376 u_var_add_root(sd, sd->base.str, true);
377 u_var_add_pose(sd, &sd->center, "center");
378 u_var_add_bool(sd, &sd->active, "active");
379
380 return &sd->base;
381}