mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-23 15:11:47 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity
monado/src/xrt/drivers/simulated/simulated_controller.c
Jakob Bornecrantz d1b7e3e557 d/simulated: Add controllers 
Supports simulating Simple, WinMR and ML2 controllers
2023-01-30 13:20:57 +00:00

408 lines
13 KiB
C
Raw Blame History

// Copyright 2020-2023, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Simulated controller device.
* @author Jakob Bornecrantz <jakob@collabora.com>
* @ingroup drv_simulated
*/
#include "xrt/xrt_device.h"
#include "os/os_time.h"
#include "math/m_api.h"
#include "math/m_mathinclude.h"
#include "util/u_var.h"
#include "util/u_misc.h"
#include "util/u_time.h"
#include "util/u_debug.h"
#include "util/u_device.h"
#include "util/u_logging.h"
#include "util/u_distortion_mesh.h"
#include "simulated_interface.h"
#include <stdio.h>
#include <assert.h>
/*
*
* Structs and defines.
*
*/
struct simulated_device
{
struct xrt_device base;
struct xrt_pose center;
bool active;
};
#define CHECK_THAT_NAME_IS_AND_ERROR(NAME) \
do { \
if (sd->base.name != NAME) { \
U_LOG_E("Unknown input for controller %s 0x%02x", #NAME, name); \
return; \
} \
} while (false)
/*
*
* Helper functions.
*
*/
static inline struct simulated_device *
simulated_device(struct xrt_device *xdev)
{
return (struct simulated_device *)xdev;
}
static const char *
device_type_to_printable_handedness(enum xrt_device_type type)
{
switch (type) {
case XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER: return " Left";
case XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER: return " Right";
default: assert(false && "Must be valid handedness"); return NULL;
}
}
/*
*
* Member functions.
*
*/
static void
simulated_device_destroy(struct xrt_device *xdev)
{
struct simulated_device *sd = simulated_device(xdev);
// Remove the variable tracking.
u_var_remove_root(sd);
// Free this device with the helper.
u_device_free(&sd->base);
}
static void
simulated_device_update_inputs(struct xrt_device *xdev)
{
struct simulated_device *sd = simulated_device(xdev);
uint64_t now = os_monotonic_get_ns();
if (!sd->active) {
for (uint32_t i = 0; i < xdev->input_count; i++) {
xdev->inputs[i].active = false;
xdev->inputs[i].timestamp = now;
U_ZERO(&xdev->inputs[i].value);
}
return;
}
for (uint32_t i = 0; i < xdev->input_count; i++) {
xdev->inputs[i].active = true;
xdev->inputs[i].timestamp = now;
}
}
static void
simulated_device_get_tracked_pose(struct xrt_device *xdev,
enum xrt_input_name name,
uint64_t at_timestamp_ns,
struct xrt_space_relation *out_relation)
{
struct simulated_device *sd = simulated_device(xdev);
switch (name) {
case XRT_INPUT_SIMPLE_GRIP_POSE:
case XRT_INPUT_SIMPLE_AIM_POSE: CHECK_THAT_NAME_IS_AND_ERROR(XRT_DEVICE_SIMPLE_CONTROLLER); break;
case XRT_INPUT_WMR_GRIP_POSE:
case XRT_INPUT_WMR_AIM_POSE: CHECK_THAT_NAME_IS_AND_ERROR(XRT_DEVICE_WMR_CONTROLLER); break;
case XRT_INPUT_ML2_CONTROLLER_GRIP_POSE:
case XRT_INPUT_ML2_CONTROLLER_AIM_POSE: CHECK_THAT_NAME_IS_AND_ERROR(XRT_DEVICE_ML2_CONTROLLER); break;
default: U_LOG_E("Unknown input name: 0x%0x", name); return;
}
if (!sd->active) {
out_relation->pose = (struct xrt_pose)XRT_POSE_IDENTITY;
out_relation->relation_flags = 0;
return;
}
struct xrt_pose pose = sd->center;
struct xrt_vec3 linear_velocity = XRT_VEC3_ZERO;
struct xrt_vec3 angular_velocity = XRT_VEC3_ZERO;
/*
* It's easier to reason about angular velocity if it's controlled in
* body space, but the angular velocity returned in the relation is in
* the base space.
*/
math_quat_rotate_derivative(&pose.orientation, &angular_velocity, &out_relation->angular_velocity);
out_relation->pose = pose;
out_relation->linear_velocity = linear_velocity;
out_relation->relation_flags = (enum xrt_space_relation_flags)(
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT |
XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT |
XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
}
static void
simulated_device_get_hand_tracking(struct xrt_device *xdev,
enum xrt_input_name name,
uint64_t requested_timestamp_ns,
struct xrt_hand_joint_set *out_value,
uint64_t *out_timestamp_ns)
{
assert(false);
}
static void
simulated_device_get_view_poses(struct xrt_device *xdev,
const struct xrt_vec3 *default_eye_relation,
uint64_t at_timestamp_ns,
uint32_t view_count,
struct xrt_space_relation *out_head_relation,
struct xrt_fov *out_fovs,
struct xrt_pose *out_poses)
{
assert(false);
}
static void
simulated_device_set_output(struct xrt_device *xdev, enum xrt_output_name name, const union xrt_output_value *value)
{
struct simulated_device *sd = simulated_device(xdev);
(void)sd;
}
/*
*
* Various data driven arrays.
*
*/
/*
* Simple Controller.
*/
static enum xrt_input_name simple_inputs_array[] = {
XRT_INPUT_SIMPLE_SELECT_CLICK,
XRT_INPUT_SIMPLE_MENU_CLICK,
XRT_INPUT_SIMPLE_GRIP_POSE,
XRT_INPUT_SIMPLE_AIM_POSE,
};
static enum xrt_output_name simple_outputs_array[] = {
XRT_OUTPUT_NAME_SIMPLE_VIBRATION,
};
/*
* WinMR Controller.
*/
static enum xrt_input_name wmr_inputs_array[] = {
XRT_INPUT_WMR_MENU_CLICK, XRT_INPUT_WMR_SQUEEZE_CLICK, XRT_INPUT_WMR_TRIGGER_VALUE,
XRT_INPUT_WMR_THUMBSTICK_CLICK, XRT_INPUT_WMR_THUMBSTICK, XRT_INPUT_WMR_TRACKPAD_CLICK,
XRT_INPUT_WMR_TRACKPAD_TOUCH, XRT_INPUT_WMR_TRACKPAD, XRT_INPUT_WMR_GRIP_POSE,
XRT_INPUT_WMR_AIM_POSE,
};
static enum xrt_output_name wmr_outputs_array[] = {
XRT_OUTPUT_NAME_WMR_HAPTIC,
};
static struct xrt_binding_input_pair wmr_to_simple_inputs[4] = {
{XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_WMR_TRIGGER_VALUE},
{XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_WMR_MENU_CLICK},
{XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_WMR_GRIP_POSE},
{XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_WMR_AIM_POSE},
};
static struct xrt_binding_output_pair wmr_to_simple_outputs[1] = {
{XRT_OUTPUT_NAME_SIMPLE_VIBRATION, XRT_OUTPUT_NAME_INDEX_HAPTIC},
};
static struct xrt_binding_profile wmr_binding_profiles[1] = {
{
.name = XRT_DEVICE_SIMPLE_CONTROLLER,
.inputs = wmr_to_simple_inputs,
.input_count = ARRAY_SIZE(wmr_to_simple_inputs),
.outputs = wmr_to_simple_outputs,
.output_count = ARRAY_SIZE(wmr_to_simple_outputs),
},
};
/*
* ML2 Controller.
*/
static enum xrt_input_name ml2_inputs_array[] = {
XRT_INPUT_ML2_CONTROLLER_MENU_CLICK, XRT_INPUT_ML2_CONTROLLER_SELECT_CLICK,
XRT_INPUT_ML2_CONTROLLER_TRIGGER_CLICK, XRT_INPUT_ML2_CONTROLLER_TRIGGER_VALUE,
XRT_INPUT_ML2_CONTROLLER_TRACKPAD_CLICK, XRT_INPUT_ML2_CONTROLLER_TRACKPAD_TOUCH,
XRT_INPUT_ML2_CONTROLLER_TRACKPAD_FORCE, XRT_INPUT_ML2_CONTROLLER_TRACKPAD,
XRT_INPUT_ML2_CONTROLLER_GRIP_POSE, XRT_INPUT_ML2_CONTROLLER_AIM_POSE,
XRT_INPUT_ML2_CONTROLLER_SHOULDER_CLICK,
};
static enum xrt_output_name ml2_outputs_array[] = {
XRT_OUTPUT_NAME_ML2_CONTROLLER_VIBRATION,
};
static struct xrt_binding_input_pair ml2_to_simple_inputs[4] = {
{XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_ML2_CONTROLLER_TRIGGER_VALUE},
{XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_ML2_CONTROLLER_MENU_CLICK},
{XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_ML2_CONTROLLER_GRIP_POSE},
{XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_ML2_CONTROLLER_AIM_POSE},
};
static struct xrt_binding_output_pair ml2_to_simple_outputs[1] = {
{XRT_OUTPUT_NAME_SIMPLE_VIBRATION, XRT_OUTPUT_NAME_INDEX_HAPTIC},
};
static struct xrt_binding_input_pair ml2_to_vive_wand_inputs[9] = {
{XRT_INPUT_VIVE_GRIP_POSE, XRT_INPUT_ML2_CONTROLLER_GRIP_POSE},
{XRT_INPUT_VIVE_AIM_POSE, XRT_INPUT_ML2_CONTROLLER_AIM_POSE},
{XRT_INPUT_VIVE_TRIGGER_CLICK, XRT_INPUT_ML2_CONTROLLER_TRIGGER_CLICK},
{XRT_INPUT_VIVE_TRIGGER_VALUE, XRT_INPUT_ML2_CONTROLLER_TRIGGER_VALUE},
{XRT_INPUT_VIVE_SQUEEZE_CLICK, XRT_INPUT_ML2_CONTROLLER_SHOULDER_CLICK},
// {XRT_INPUT_VIVE_SYSTEM_CLICK, NONE},
{XRT_INPUT_VIVE_MENU_CLICK, XRT_INPUT_ML2_CONTROLLER_MENU_CLICK},
{XRT_INPUT_VIVE_TRACKPAD, XRT_INPUT_ML2_CONTROLLER_TRACKPAD},
// {NONE, XRT_INPUT_ML2_CONTROLLER_TRACKPAD_FORCE},
{XRT_INPUT_VIVE_TRACKPAD_TOUCH, XRT_INPUT_ML2_CONTROLLER_TRACKPAD_TOUCH},
{XRT_INPUT_VIVE_TRACKPAD_CLICK, XRT_INPUT_ML2_CONTROLLER_TRACKPAD_CLICK},
};
static struct xrt_binding_output_pair ml2_to_vive_wand_outputs[1] = {
{XRT_OUTPUT_NAME_VIVE_HAPTIC, XRT_OUTPUT_NAME_ML2_CONTROLLER_VIBRATION},
};
static struct xrt_binding_profile ml2_binding_profiles[2] = {
{
.name = XRT_DEVICE_SIMPLE_CONTROLLER,
.inputs = ml2_to_simple_inputs,
.input_count = ARRAY_SIZE(ml2_to_simple_inputs),
.outputs = ml2_to_simple_outputs,
.output_count = ARRAY_SIZE(ml2_to_simple_outputs),
},
{
.name = XRT_DEVICE_VIVE_WAND,
.inputs = ml2_to_vive_wand_inputs,
.input_count = ARRAY_SIZE(ml2_to_vive_wand_inputs),
.outputs = ml2_to_vive_wand_outputs,
.output_count = ARRAY_SIZE(ml2_to_vive_wand_outputs),
},
};
/*
*
* 'Exported' functions.
*
*/
struct xrt_device *
simulated_create_controller(enum xrt_device_name name,
enum xrt_device_type type,
const struct xrt_pose *center,
struct xrt_tracking_origin *origin)
{
const enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE;
const char *handedness = "";
const char *name_str = NULL;
enum xrt_input_name *inputs = NULL;
uint32_t input_count = 0;
enum xrt_output_name *outputs = NULL;
uint32_t output_count = 0;
struct xrt_binding_profile *binding_profiles = NULL;
uint32_t binding_profile_count = 0;
switch (name) {
case XRT_DEVICE_SIMPLE_CONTROLLER:
name_str = "Simple";
input_count = ARRAY_SIZE(simple_inputs_array);
output_count = ARRAY_SIZE(simple_outputs_array);
inputs = simple_inputs_array;
outputs = simple_outputs_array;
assert(type == XRT_DEVICE_TYPE_ANY_HAND_CONTROLLER);
break;
case XRT_DEVICE_WMR_CONTROLLER:
name_str = "WinMR";
input_count = ARRAY_SIZE(wmr_inputs_array);
output_count = ARRAY_SIZE(wmr_outputs_array);
inputs = wmr_inputs_array;
outputs = wmr_outputs_array;
binding_profiles = wmr_binding_profiles;
binding_profile_count = ARRAY_SIZE(wmr_binding_profiles);
handedness = device_type_to_printable_handedness(type);
break;
case XRT_DEVICE_ML2_CONTROLLER:
name_str = "ML2";
input_count = ARRAY_SIZE(ml2_inputs_array);
output_count = ARRAY_SIZE(ml2_outputs_array);
inputs = ml2_inputs_array;
outputs = ml2_outputs_array;
binding_profiles = ml2_binding_profiles;
binding_profile_count = ARRAY_SIZE(ml2_binding_profiles);
assert(type == XRT_DEVICE_TYPE_ANY_HAND_CONTROLLER);
break;
default: assert(false); return NULL;
}
// Allocate.
struct simulated_device *sd = U_DEVICE_ALLOCATE(struct simulated_device, flags, input_count, output_count);
sd->base.update_inputs = simulated_device_update_inputs;
sd->base.get_tracked_pose = simulated_device_get_tracked_pose;
sd->base.get_hand_tracking = simulated_device_get_hand_tracking;
sd->base.get_view_poses = simulated_device_get_view_poses;
sd->base.set_output = simulated_device_set_output;
sd->base.destroy = simulated_device_destroy;
sd->base.tracking_origin = origin;
sd->base.orientation_tracking_supported = true;
sd->base.position_tracking_supported = true;
sd->base.hand_tracking_supported = false;
sd->base.name = name;
sd->base.device_type = type;
sd->base.binding_profiles = binding_profiles;
sd->base.binding_profile_count = binding_profile_count;
snprintf(sd->base.str, sizeof(sd->base.str), "%s%s Controller (Simulated)", name_str, handedness);
snprintf(sd->base.serial, sizeof(sd->base.str), "%s%s Controller (Simulated)", name_str, handedness);
for (uint32_t i = 0; i < input_count; i++) {
sd->base.inputs[i].active = true;
sd->base.inputs[i].name = inputs[i];
}
for (uint32_t i = 0; i < output_count; i++) {
sd->base.outputs[i].name = outputs[i];
}
sd->center = *center;
sd->active = true;
u_var_add_root(sd, sd->base.str, true);
u_var_add_pose(sd, &sd->center, "center");
u_var_add_bool(sd, &sd->active, "active");
return &sd->base;
}
Reference in a new issue View git blame Copy permalink