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