diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index 3ccbf63ff..3cc411606 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -49,8 +49,8 @@ endif() if(XRT_BUILD_DRIVER_SIMULATED) add_library( - drv_simulated STATIC simulated/simulated_hmd.c simulated/simulated_interface.h - simulated/simulated_prober.c + drv_simulated STATIC simulated/simulated_controller.c simulated/simulated_hmd.c + simulated/simulated_interface.h simulated/simulated_prober.c ) target_link_libraries(drv_simulated PRIVATE xrt-interfaces aux_util) list(APPEND ENABLED_HEADSET_DRIVERS simulated) diff --git a/src/xrt/drivers/simulated/simulated_controller.c b/src/xrt/drivers/simulated/simulated_controller.c new file mode 100644 index 000000000..1e395d040 --- /dev/null +++ b/src/xrt/drivers/simulated/simulated_controller.c @@ -0,0 +1,407 @@ +// Copyright 2020-2023, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Simulated controller device. + * @author Jakob Bornecrantz + * @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 +#include + + +/* + * + * 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; +} diff --git a/src/xrt/drivers/simulated/simulated_interface.h b/src/xrt/drivers/simulated/simulated_interface.h index 5fabedcb5..410ef098e 100644 --- a/src/xrt/drivers/simulated/simulated_interface.h +++ b/src/xrt/drivers/simulated/simulated_interface.h @@ -67,6 +67,27 @@ simulated_create_auto_prober(void); struct xrt_device * simulated_hmd_create(enum simulated_movement movement, const struct xrt_pose *center); +/*! + * Create a simulated controller. + * + * Supported configs: + * + * * XRT_DEVICE_SIMPLE_CONTROLLER + * * XRT_DEVICE_TYPE_ANY_CONTROLLER + * * XRT_DEVICE_WMR_CONTROLLER + * * XRT_DEVICE_TYPE_LEFT_CONTROLLER + * * XRT_DEVICE_TYPE_RIGHT_CONTROLLER + * * XRT_DEVICE_ML2_CONTROLLER + * * XRT_DEVICE_TYPE_ANY_CONTROLLER + * + * @ingroup drv_simulated + */ +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); + #ifdef __cplusplus }