diff --git a/examples/xr.rs b/examples/xr.rs index 548fe15..8644265 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,11 +1,24 @@ + + use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; + use bevy::prelude::*; use bevy::transform::components::Transform; -use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; +use bevy_openxr::input::XrInput; +use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; + +use bevy_openxr::xr_input::hand::{OpenXrHandInput, HandInputDebugRenderer}; +use bevy_openxr::xr_input::interactions::{ + draw_interaction_gizmos, draw_socket_gizmos, interactions, socket_interactions, + update_interactable_states, InteractionEvent, Touched, XRDirectInteractor, XRInteractable, + XRInteractableState, XRInteractorState, XRRayInteractor, XRSocketInteractor, +}; +use bevy_openxr::xr_input::oculus_touch::OculusController; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ - OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, + AimPose, OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, }; +use bevy_openxr::xr_input::Hand; use bevy_openxr::DefaultXrPlugins; fn main() { @@ -14,13 +27,29 @@ fn main() { info!("Running `openxr-6dof` skill"); App::new() .add_plugins(DefaultXrPlugins) - .add_plugins(OpenXrDebugRenderer) //new debug renderer adds gizmos to + //.add_plugins(OpenXrDebugRenderer) //new debug renderer adds gizmos to .add_plugins(LogDiagnosticsPlugin::default()) .add_plugins(FrameTimeDiagnosticsPlugin) .add_systems(Startup, setup) .add_systems(Update, proto_locomotion) - .add_systems(Startup, spawn_controllers_example) .insert_resource(PrototypeLocomotionConfig::default()) + .add_systems(Startup, spawn_controllers_example) + .add_plugins(OpenXrHandInput) + .add_plugins(HandInputDebugRenderer) + .add_systems( + Update, + draw_interaction_gizmos.after(update_interactable_states), + ) + .add_systems(Update, draw_socket_gizmos.after(update_interactable_states)) + .add_systems(Update, interactions.before(update_interactable_states)) + .add_systems( + Update, + socket_interactions.before(update_interactable_states), + ) + .add_systems(Update, prototype_interaction_input) + .add_systems(Update, update_interactable_states) + .add_systems(Update, update_grabbables.after(update_interactable_states)) + .add_event::() .run(); } @@ -43,13 +72,16 @@ fn setup( transform: Transform::from_xyz(0.0, 0.5, 0.0), ..default() }); - // cube - commands.spawn(PbrBundle { - mesh: meshes.add(Mesh::from(shape::Cube { size: 0.1 })), - material: materials.add(Color::rgb(0.8, 0.0, 0.0).into()), - transform: Transform::from_xyz(0.0, 0.5, 1.0), - ..default() - }); + // socket + commands.spawn(( + SpatialBundle { + transform: Transform::from_xyz(0.0, 0.5, 1.0), + ..default() + }, + XRInteractorState::Selecting, + XRSocketInteractor, + )); + // light commands.spawn(PointLightBundle { point_light: PointLight { @@ -62,9 +94,27 @@ fn setup( }); // camera commands.spawn((Camera3dBundle { - transform: Transform::from_xyz(-2.0, 2.5, 5.0).looking_at(Vec3::ZERO, Vec3::Y), + transform: Transform::from_xyz(0.25, 1.25, 0.0).looking_at( + Vec3 { + x: -0.548, + y: -0.161, + z: -0.137, + }, + Vec3::Y, + ), ..default() },)); + //simple interactable + commands.spawn(( + SpatialBundle { + transform: Transform::from_xyz(0.0, 1.0, 0.0), + ..default() + }, + XRInteractable, + XRInteractableState::default(), + Grabbable, + Touched(false), + )); } fn spawn_controllers_example(mut commands: Commands) { @@ -74,6 +124,9 @@ fn spawn_controllers_example(mut commands: Commands) { OpenXRController, OpenXRTracker, SpatialBundle::default(), + XRRayInteractor, + AimPose(Transform::default()), + XRInteractorState::default(), )); //right hand commands.spawn(( @@ -81,5 +134,89 @@ fn spawn_controllers_example(mut commands: Commands) { OpenXRController, OpenXRTracker, SpatialBundle::default(), + XRDirectInteractor, + XRInteractorState::default(), )); } + +fn prototype_interaction_input( + oculus_controller: Res, + frame_state: Res, + xr_input: Res, + instance: Res, + session: Res, + mut right_interactor_query: Query< + (&mut XRInteractorState), + ( + With, + With, + Without, + ), + >, + mut left_interactor_query: Query< + (&mut XRInteractorState), + ( + With, + With, + Without, + ), + >, +) { + //lock frame + let frame_state = *frame_state.lock().unwrap(); + //get controller + let controller = oculus_controller.get_ref(&instance, &session, &frame_state, &xr_input); + //get controller triggers + let left_trigger = controller.trigger(Hand::Left); + let right_trigger = controller.trigger(Hand::Right); + //get the interactors and do state stuff + let mut left_state = left_interactor_query.single_mut(); + if left_trigger > 0.8 { + *left_state = XRInteractorState::Selecting; + } else { + *left_state = XRInteractorState::Idle; + } + let mut right_state = right_interactor_query.single_mut(); + if right_trigger > 0.8 { + *right_state = XRInteractorState::Selecting; + } else { + *right_state = XRInteractorState::Idle; + } +} + +#[derive(Component)] +pub struct Grabbable; + +pub fn update_grabbables( + mut events: EventReader, + mut grabbable_query: Query<(&mut Transform, With, Without)>, + interactor_query: Query<(&GlobalTransform, &XRInteractorState, Without)>, +) { + //so basically the idea is to try all the events? + for event in events.read() { + // info!("some event"); + match grabbable_query.get_mut(event.interactable) { + Ok(mut grabbable_transform) => { + // info!("we got a grabbable"); + //now we need the location of our interactor + match interactor_query.get(event.interactor) { + Ok(interactor_transform) => { + match interactor_transform.1 { + XRInteractorState::Idle => (), + XRInteractorState::Selecting => { + // info!("its a direct interactor?"); + *grabbable_transform.0 = interactor_transform.0.compute_transform(); + } + } + } + Err(_) => { + // info!("not a direct interactor") + } + } + } + Err(_) => { + // info!("not a grabbable?") + } + } + } +} diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs new file mode 100644 index 0000000..81f5936 --- /dev/null +++ b/src/xr_input/hand.rs @@ -0,0 +1,1095 @@ +use std::f32::consts::PI; + +use bevy::prelude::{ + default, info, Color, Commands, Component, Entity, Gizmos, GlobalTransform, Plugin, PostUpdate, + PreUpdate, Quat, Query, Res, ResMut, Resource, SpatialBundle, Startup, Transform, Update, Vec3, + With, +}; +use openxr::{HandJoint, Posef}; + +use crate::{ + input::XrInput, + resources::{XrFrameState, XrInstance, XrSession}, + xr_input::Vec3Conv, +}; + +use super::{ + hand_poses::get_simulated_open_hand_transforms, + oculus_touch::OculusController, + trackers::{OpenXRLeftController, OpenXRRightController, OpenXRTracker}, + Hand, +}; + +/// add debug renderer for controllers +#[derive(Default)] +pub struct OpenXrHandInput; + +impl Plugin for OpenXrHandInput { + fn build(&self, app: &mut bevy::prelude::App) { + app.add_systems(Update, update_hand_skeletons) + .add_systems(PreUpdate, update_hand_states) + .add_systems(Startup, spawn_hand_entities) + .insert_resource(HandStatesResource::default()) + .insert_resource(HandInputSource::default()); + } +} + +/// add debug renderer for controllers +#[derive(Default)] +pub struct HandInputDebugRenderer; + +impl Plugin for HandInputDebugRenderer{ + fn build(&self, app: &mut bevy::prelude::App) { + app.add_systems(PostUpdate, draw_hand_entities); + } +} + +#[derive(Resource)] +pub enum HandInputSource { + Emulated, + OpenXr, +} + +impl Default for HandInputSource { + fn default() -> Self { + HandInputSource::Emulated + } +} + +#[derive(Resource, Default)] +pub struct HandsResource { + pub left: HandResource, + pub right: HandResource, +} + +pub struct HandResource { + pub palm: Entity, + pub wrist: Entity, + pub thumb: ThumbResource, + pub index: IndexResource, + pub middle: MiddleResource, + pub ring: RingResource, + pub little: LittleResource, +} + +impl Default for HandResource { + fn default() -> Self { + Self { + palm: Entity::PLACEHOLDER, + wrist: Entity::PLACEHOLDER, + thumb: Default::default(), + index: Default::default(), + middle: Default::default(), + ring: Default::default(), + little: Default::default(), + } + } +} + +pub struct ThumbResource { + pub metacarpal: Entity, + pub proximal: Entity, + pub distal: Entity, + pub tip: Entity, +} + +impl Default for ThumbResource { + fn default() -> Self { + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } + } +} +pub struct IndexResource { + pub metacarpal: Entity, + pub proximal: Entity, + pub intermediate: Entity, + pub distal: Entity, + pub tip: Entity, +} + +impl Default for IndexResource { + fn default() -> Self { + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + intermediate: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } + } +} +pub struct MiddleResource { + pub metacarpal: Entity, + pub proximal: Entity, + pub intermediate: Entity, + pub distal: Entity, + pub tip: Entity, +} +impl Default for MiddleResource { + fn default() -> Self { + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + intermediate: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } + } +} +pub struct RingResource { + pub metacarpal: Entity, + pub proximal: Entity, + pub intermediate: Entity, + pub distal: Entity, + pub tip: Entity, +} +impl Default for RingResource { + fn default() -> Self { + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + intermediate: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } + } +} +pub struct LittleResource { + pub metacarpal: Entity, + pub proximal: Entity, + pub intermediate: Entity, + pub distal: Entity, + pub tip: Entity, +} +impl Default for LittleResource { + fn default() -> Self { + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + intermediate: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } + } +} + +pub fn spawn_hand_entities(mut commands: Commands) { + let hands = [Hand::Left, Hand::Right]; + let bones = [ + HandBone::Palm, + HandBone::Wrist, + HandBone::ThumbMetacarpal, + HandBone::ThumbProximal, + HandBone::ThumbDistal, + HandBone::ThumbTip, + HandBone::IndexMetacarpal, + HandBone::IndexProximal, + HandBone::IndexIntermediate, + HandBone::IndexDistal, + HandBone::IndexTip, + HandBone::MiddleMetacarpal, + HandBone::MiddleProximal, + HandBone::MiddleIntermediate, + HandBone::MiddleDistal, + HandBone::MiddleTip, + HandBone::RingMetacarpal, + HandBone::RingProximal, + HandBone::RingIntermediate, + HandBone::RingDistal, + HandBone::RingTip, + HandBone::LittleMetacarpal, + HandBone::LittleProximal, + HandBone::LittleIntermediate, + HandBone::LittleDistal, + HandBone::LittleTip, + ]; + //hand resource + let mut hand_resource = HandsResource { ..default() }; + for hand in hands.iter() { + for bone in bones.iter() { + let boneid = commands + .spawn(( + SpatialBundle::default(), + bone.clone(), + OpenXRTracker, + hand.clone(), + )) + .id(); + match hand { + Hand::Left => match bone { + HandBone::Palm => hand_resource.left.palm = boneid, + HandBone::Wrist => hand_resource.left.wrist = boneid, + HandBone::ThumbMetacarpal => hand_resource.left.thumb.metacarpal = boneid, + HandBone::ThumbProximal => hand_resource.left.thumb.proximal = boneid, + HandBone::ThumbDistal => hand_resource.left.thumb.distal = boneid, + HandBone::ThumbTip => hand_resource.left.thumb.tip = boneid, + HandBone::IndexMetacarpal => hand_resource.left.index.metacarpal = boneid, + HandBone::IndexProximal => hand_resource.left.index.proximal = boneid, + HandBone::IndexIntermediate => hand_resource.left.index.intermediate = boneid, + HandBone::IndexDistal => hand_resource.left.index.distal = boneid, + HandBone::IndexTip => hand_resource.left.index.tip = boneid, + HandBone::MiddleMetacarpal => hand_resource.left.middle.metacarpal = boneid, + HandBone::MiddleProximal => hand_resource.left.middle.proximal = boneid, + HandBone::MiddleIntermediate => hand_resource.left.middle.intermediate = boneid, + HandBone::MiddleDistal => hand_resource.left.middle.distal = boneid, + HandBone::MiddleTip => hand_resource.left.middle.tip = boneid, + HandBone::RingMetacarpal => hand_resource.left.ring.metacarpal = boneid, + HandBone::RingProximal => hand_resource.left.ring.proximal = boneid, + HandBone::RingIntermediate => hand_resource.left.ring.intermediate = boneid, + HandBone::RingDistal => hand_resource.left.ring.distal = boneid, + HandBone::RingTip => hand_resource.left.ring.tip = boneid, + HandBone::LittleMetacarpal => hand_resource.left.little.metacarpal = boneid, + HandBone::LittleProximal => hand_resource.left.little.proximal = boneid, + HandBone::LittleIntermediate => hand_resource.left.little.intermediate = boneid, + HandBone::LittleDistal => hand_resource.left.little.distal = boneid, + HandBone::LittleTip => hand_resource.left.little.tip = boneid, + }, + Hand::Right => match bone { + HandBone::Palm => hand_resource.right.palm = boneid, + HandBone::Wrist => hand_resource.right.wrist = boneid, + HandBone::ThumbMetacarpal => hand_resource.right.thumb.metacarpal = boneid, + HandBone::ThumbProximal => hand_resource.right.thumb.proximal = boneid, + HandBone::ThumbDistal => hand_resource.right.thumb.distal = boneid, + HandBone::ThumbTip => hand_resource.right.thumb.tip = boneid, + HandBone::IndexMetacarpal => hand_resource.right.index.metacarpal = boneid, + HandBone::IndexProximal => hand_resource.right.index.proximal = boneid, + HandBone::IndexIntermediate => hand_resource.right.index.intermediate = boneid, + HandBone::IndexDistal => hand_resource.right.index.distal = boneid, + HandBone::IndexTip => hand_resource.right.index.tip = boneid, + HandBone::MiddleMetacarpal => hand_resource.right.middle.metacarpal = boneid, + HandBone::MiddleProximal => hand_resource.right.middle.proximal = boneid, + HandBone::MiddleIntermediate => { + hand_resource.right.middle.intermediate = boneid + } + HandBone::MiddleDistal => hand_resource.right.middle.distal = boneid, + HandBone::MiddleTip => hand_resource.right.middle.tip = boneid, + HandBone::RingMetacarpal => hand_resource.right.ring.metacarpal = boneid, + HandBone::RingProximal => hand_resource.right.ring.proximal = boneid, + HandBone::RingIntermediate => hand_resource.right.ring.intermediate = boneid, + HandBone::RingDistal => hand_resource.right.ring.distal = boneid, + HandBone::RingTip => hand_resource.right.ring.tip = boneid, + HandBone::LittleMetacarpal => hand_resource.right.little.metacarpal = boneid, + HandBone::LittleProximal => hand_resource.right.little.proximal = boneid, + HandBone::LittleIntermediate => { + hand_resource.right.little.intermediate = boneid + } + HandBone::LittleDistal => hand_resource.right.little.distal = boneid, + HandBone::LittleTip => hand_resource.right.little.tip = boneid, + }, + } + } + } + commands.insert_resource(hand_resource); +} + +#[derive(Component, Debug, Clone, Copy)] +pub enum HandBone { + Palm, + Wrist, + ThumbMetacarpal, + ThumbProximal, + ThumbDistal, + ThumbTip, + IndexMetacarpal, + IndexProximal, + IndexIntermediate, + IndexDistal, + IndexTip, + MiddleMetacarpal, + MiddleProximal, + MiddleIntermediate, + MiddleDistal, + MiddleTip, + RingMetacarpal, + RingProximal, + RingIntermediate, + RingDistal, + RingTip, + LittleMetacarpal, + LittleProximal, + LittleIntermediate, + LittleDistal, + LittleTip, +} + +pub fn update_hand_states( + oculus_controller: Res, + hand_states_option: Option>, + frame_state: Res, + xr_input: Res, + instance: Res, + session: Res, +) { + match hand_states_option { + Some(mut hands) => { + //lock frame + let frame_state = *frame_state.lock().unwrap(); + //get controller + let controller = + oculus_controller.get_ref(&instance, &session, &frame_state, &xr_input); + + //right hand + let squeeze = controller.squeeze(Hand::Right); + let trigger_state = controller.trigger(Hand::Right); + let calc_trigger_state = match controller.trigger_touched(Hand::Right) { + true => match trigger_state > 0.0 { + true => TriggerState::PULLED, + false => TriggerState::TOUCHED, + }, + false => TriggerState::OFF, + }; + //button a + let mut a_state = ButtonState::OFF; + if controller.a_button_touched() { + a_state = ButtonState::TOUCHED; + } + if controller.a_button() { + a_state = ButtonState::PRESSED; + } + + //button b + let mut b_state = ButtonState::OFF; + if controller.b_button_touched() { + b_state = ButtonState::TOUCHED; + } + if controller.b_button() { + b_state = ButtonState::PRESSED; + } + + let thumbstick_state = controller.thumbstick(Hand::Right); + let calc_thumbstick_state = match controller.thumbstick_touch(Hand::Right) { + true => match thumbstick_state.x > 0.0 || thumbstick_state.y > 0.0 { + true => ThumbstickState::PRESSED, + false => ThumbstickState::TOUCHED, + }, + false => ThumbstickState::OFF, + }; + + let right_state = HandState { + grip: squeeze, + trigger_state: calc_trigger_state, + a_button: a_state, + b_button: b_state, + thumbstick: calc_thumbstick_state, + }; + + //left + let squeeze = controller.squeeze(Hand::Left); + let trigger_state = controller.trigger(Hand::Left); + let calc_trigger_state = match controller.trigger_touched(Hand::Left) { + true => match trigger_state > 0.0 { + true => TriggerState::PULLED, + false => TriggerState::TOUCHED, + }, + false => TriggerState::OFF, + }; + //button a + let mut a_state = ButtonState::OFF; + if controller.x_button_touched() { + a_state = ButtonState::TOUCHED; + } + if controller.x_button() { + a_state = ButtonState::PRESSED; + } + + //button b + let mut b_state = ButtonState::OFF; + if controller.y_button_touched() { + b_state = ButtonState::TOUCHED; + } + if controller.y_button() { + b_state = ButtonState::PRESSED; + } + + let thumbstick_state = controller.thumbstick(Hand::Left); + let calc_thumbstick_state = match controller.thumbstick_touch(Hand::Left) { + true => match thumbstick_state.x > 0.0 || thumbstick_state.y > 0.0 { + true => ThumbstickState::PRESSED, + false => ThumbstickState::TOUCHED, + }, + false => ThumbstickState::OFF, + }; + + let left_state = HandState { + grip: squeeze, + trigger_state: calc_trigger_state, + a_button: a_state, + b_button: b_state, + thumbstick: calc_thumbstick_state, + }; + + hands.left = left_state; + hands.right = right_state; + } + None => info!("hand states resource not init yet"), + } +} + +#[derive(Clone, Copy)] +pub enum ButtonState { + OFF, + TOUCHED, + PRESSED, +} + +impl Default for ButtonState { + fn default() -> Self { + ButtonState::OFF + } +} +#[derive(Clone, Copy)] +pub enum ThumbstickState { + OFF, + TOUCHED, + PRESSED, +} + +impl Default for ThumbstickState { + fn default() -> Self { + ThumbstickState::OFF + } +} +#[derive(Clone, Copy)] +pub enum TriggerState { + OFF, + TOUCHED, + PULLED, +} + +impl Default for TriggerState { + fn default() -> Self { + TriggerState::OFF + } +} + +#[derive(Default, Resource)] +pub struct HandStatesResource { + pub left: HandState, + pub right: HandState, +} + +#[derive(Clone, Copy)] +pub struct HandState { + pub grip: f32, + pub trigger_state: TriggerState, + pub a_button: ButtonState, + pub b_button: ButtonState, + pub thumbstick: ThumbstickState, +} + +impl HandState { + pub fn get_index_curl(&self) -> f32 { + match self.trigger_state { + TriggerState::OFF => 0.0, + TriggerState::TOUCHED => 0.50, + TriggerState::PULLED => 1.0, + } + } + + pub fn get_thumb_curl(&self) -> f32 { + match self.thumbstick { + ThumbstickState::OFF => (), + ThumbstickState::TOUCHED => return 0.25, + ThumbstickState::PRESSED => return 0.25, + }; + + match self.a_button { + ButtonState::OFF => (), + ButtonState::TOUCHED => return 0.25, + ButtonState::PRESSED => return 0.25, + }; + + match self.b_button { + ButtonState::OFF => (), + ButtonState::TOUCHED => return 0.25, + ButtonState::PRESSED => return 0.25, + }; + //if no thumb actions taken return open position + return 0.0; + } +} + +impl Default for HandState { + fn default() -> Self { + Self { + grip: Default::default(), + trigger_state: Default::default(), + a_button: Default::default(), + b_button: Default::default(), + thumbstick: Default::default(), + } + } +} + +pub fn update_hand_bones_emulated( + controller_transform: Transform, + hand: Hand, + hand_state: HandState, + hand_bone_query: &mut Query<(&mut Transform, &HandBone, &Hand)>, +) { + let left_hand_rot = Quat::from_rotation_y(180.0 * PI / 180.0); + let hand_translation: Vec3 = match hand { + Hand::Left => controller_transform.translation, + Hand::Right => controller_transform.translation, + }; + + let controller_quat: Quat = match hand { + Hand::Left => controller_transform.rotation.mul_quat(left_hand_rot), + Hand::Right => controller_transform.rotation, + }; + + let splay_direction = match hand { + Hand::Left => -1.0, + Hand::Right => 1.0, + }; + //lets make a structure to hold our calculated transforms for now + let mut calc_transforms = [Transform::default(); 26]; + + //curl represents how closed the hand is from 0 to 1; + let grip_curl = hand_state.grip; + let index_curl = hand_state.get_index_curl(); + let thumb_curl = hand_state.get_thumb_curl(); + //get palm quat + let y = Quat::from_rotation_y(-90.0 * PI / 180.0); + let x = Quat::from_rotation_x(-90.0 * PI / 180.0); + let palm_quat = controller_quat.mul_quat(y).mul_quat(x); + //get simulated bones + let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); + //palm + let palm = hand_transform_array[HandJoint::PALM]; + calc_transforms[HandJoint::PALM] = Transform { + translation: hand_translation + palm.translation, + ..default() + }; + //wrist + let wrist = hand_transform_array[HandJoint::WRIST]; + calc_transforms[HandJoint::WRIST] = Transform { + translation: hand_translation + palm.translation + palm_quat.mul_vec3(wrist.translation), + ..default() + }; + + //thumb + let thumb_joints = [ + HandJoint::THUMB_METACARPAL, + HandJoint::THUMB_PROXIMAL, + HandJoint::THUMB_DISTAL, + HandJoint::THUMB_TIP, + ]; + let mut prior_start: Option = None; + let mut prior_quat: Option = None; + let mut prior_vector: Option = None; + let splay = Quat::from_rotation_y(splay_direction * 30.0 * PI / 180.0); + let huh = Quat::from_rotation_x(-35.0 * PI / 180.0); + let splay_quat = palm_quat.mul_quat(huh).mul_quat(splay); + for bone in thumb_joints.iter() { + match prior_start { + Some(start) => { + let curl_angle: f32 = get_bone_curl_angle(*bone, thumb_curl); + let tp_lrot = Quat::from_rotation_y(splay_direction * curl_angle * PI / 180.0); + let tp_quat = prior_quat.unwrap().mul_quat(tp_lrot); + let thumb_prox = hand_transform_array[*bone]; + let tp_start = start + prior_vector.unwrap(); + let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + //store it + calc_transforms[*bone] = Transform { + translation: tp_start + tp_vector, + ..default() + }; + } + None => { + let thumb_meta = hand_transform_array[*bone]; + let tm_start = hand_translation + + palm_quat.mul_vec3(palm.translation) + + palm_quat.mul_vec3(wrist.translation); + let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + //store it + calc_transforms[*bone] = Transform { + translation: tm_start + tm_vector, + ..default() + }; + } + } + } + + //index + let thumb_joints = [ + HandJoint::INDEX_METACARPAL, + HandJoint::INDEX_PROXIMAL, + HandJoint::INDEX_INTERMEDIATE, + HandJoint::INDEX_DISTAL, + HandJoint::INDEX_TIP, + ]; + let mut prior_start: Option = None; + let mut prior_quat: Option = None; + let mut prior_vector: Option = None; + let splay = Quat::from_rotation_y(splay_direction * 10.0 * PI / 180.0); + let splay_quat = palm_quat.mul_quat(splay); + for bone in thumb_joints.iter() { + match prior_start { + Some(start) => { + let curl_angle: f32 = get_bone_curl_angle(*bone, index_curl); + let tp_lrot = Quat::from_rotation_x(curl_angle * PI / 180.0); + let tp_quat = prior_quat.unwrap().mul_quat(tp_lrot); + let thumb_prox = hand_transform_array[*bone]; + let tp_start = start + prior_vector.unwrap(); + let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + //store it + calc_transforms[*bone] = Transform { + translation: tp_start + tp_vector, + ..default() + }; + } + None => { + let thumb_meta = hand_transform_array[*bone]; + let tm_start = hand_translation + + palm_quat.mul_vec3(palm.translation) + + palm_quat.mul_vec3(wrist.translation); + let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + //store it + calc_transforms[*bone] = Transform { + translation: tm_start + tm_vector, + ..default() + }; + } + } + } + + //middle + let thumb_joints = [ + HandJoint::MIDDLE_METACARPAL, + HandJoint::MIDDLE_PROXIMAL, + HandJoint::MIDDLE_INTERMEDIATE, + HandJoint::MIDDLE_DISTAL, + HandJoint::MIDDLE_TIP, + ]; + let mut prior_start: Option = None; + let mut prior_quat: Option = None; + let mut prior_vector: Option = None; + let splay = Quat::from_rotation_y(splay_direction * 0.0 * PI / 180.0); + let splay_quat = palm_quat.mul_quat(splay); + for bone in thumb_joints.iter() { + match prior_start { + Some(start) => { + let curl_angle: f32 = get_bone_curl_angle(*bone, grip_curl); + let tp_lrot = Quat::from_rotation_x(curl_angle * PI / 180.0); + let tp_quat = prior_quat.unwrap().mul_quat(tp_lrot); + let thumb_prox = hand_transform_array[*bone]; + let tp_start = start + prior_vector.unwrap(); + let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + //store it + calc_transforms[*bone] = Transform { + translation: tp_start + tp_vector, + ..default() + }; + } + None => { + let thumb_meta = hand_transform_array[*bone]; + let tm_start = hand_translation + + palm_quat.mul_vec3(palm.translation) + + palm_quat.mul_vec3(wrist.translation); + let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + //store it + calc_transforms[*bone] = Transform { + translation: tm_start + tm_vector, + ..default() + }; + } + } + } + //ring + let thumb_joints = [ + HandJoint::RING_METACARPAL, + HandJoint::RING_PROXIMAL, + HandJoint::RING_INTERMEDIATE, + HandJoint::RING_DISTAL, + HandJoint::RING_TIP, + ]; + let mut prior_start: Option = None; + let mut prior_quat: Option = None; + let mut prior_vector: Option = None; + let splay = Quat::from_rotation_y(splay_direction * -10.0 * PI / 180.0); + let splay_quat = palm_quat.mul_quat(splay); + for bone in thumb_joints.iter() { + match prior_start { + Some(start) => { + let curl_angle: f32 = get_bone_curl_angle(*bone, grip_curl); + let tp_lrot = Quat::from_rotation_x(curl_angle * PI / 180.0); + let tp_quat = prior_quat.unwrap().mul_quat(tp_lrot); + let thumb_prox = hand_transform_array[*bone]; + let tp_start = start + prior_vector.unwrap(); + let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + //store it + calc_transforms[*bone] = Transform { + translation: tp_start + tp_vector, + ..default() + }; + } + None => { + let thumb_meta = hand_transform_array[*bone]; + let tm_start = hand_translation + + palm_quat.mul_vec3(palm.translation) + + palm_quat.mul_vec3(wrist.translation); + let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + //store it + calc_transforms[*bone] = Transform { + translation: tm_start + tm_vector, + ..default() + }; + } + } + } + + //little + let thumb_joints = [ + HandJoint::LITTLE_METACARPAL, + HandJoint::LITTLE_PROXIMAL, + HandJoint::LITTLE_INTERMEDIATE, + HandJoint::LITTLE_DISTAL, + HandJoint::LITTLE_TIP, + ]; + let mut prior_start: Option = None; + let mut prior_quat: Option = None; + let mut prior_vector: Option = None; + let splay = Quat::from_rotation_y(splay_direction * -20.0 * PI / 180.0); + let splay_quat = palm_quat.mul_quat(splay); + for bone in thumb_joints.iter() { + match prior_start { + Some(start) => { + let curl_angle: f32 = get_bone_curl_angle(*bone, grip_curl); + let tp_lrot = Quat::from_rotation_x(curl_angle * PI / 180.0); + let tp_quat = prior_quat.unwrap().mul_quat(tp_lrot); + let thumb_prox = hand_transform_array[*bone]; + let tp_start = start + prior_vector.unwrap(); + let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + //store it + calc_transforms[*bone] = Transform { + translation: tp_start + tp_vector, + ..default() + }; + } + None => { + let thumb_meta = hand_transform_array[*bone]; + let tm_start = hand_translation + + palm_quat.mul_vec3(palm.translation) + + palm_quat.mul_vec3(wrist.translation); + let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + //store it + calc_transforms[*bone] = Transform { + translation: tm_start + tm_vector, + ..default() + }; + } + } + } + + //now that we have all the transforms lets assign them + for (mut transform, handbone, bonehand) in hand_bone_query.iter_mut() { + if *bonehand == hand { + //if the hands match lets go + let index = match_index(handbone); + *transform = calc_transforms[index]; + } + } +} + +fn match_index(handbone: &HandBone) -> HandJoint { + match handbone { + HandBone::Palm => HandJoint::PALM, + HandBone::Wrist => HandJoint::WRIST, + HandBone::ThumbMetacarpal => HandJoint::THUMB_METACARPAL, + HandBone::ThumbProximal => HandJoint::THUMB_PROXIMAL, + HandBone::ThumbDistal => HandJoint::THUMB_DISTAL, + HandBone::ThumbTip => HandJoint::THUMB_TIP, + HandBone::IndexMetacarpal => HandJoint::INDEX_METACARPAL, + HandBone::IndexProximal => HandJoint::INDEX_PROXIMAL, + HandBone::IndexIntermediate => HandJoint::INDEX_INTERMEDIATE, + HandBone::IndexDistal => HandJoint::INDEX_DISTAL, + HandBone::IndexTip => HandJoint::INDEX_TIP, + HandBone::MiddleMetacarpal => HandJoint::MIDDLE_METACARPAL, + HandBone::MiddleProximal => HandJoint::MIDDLE_PROXIMAL, + HandBone::MiddleIntermediate => HandJoint::MIDDLE_INTERMEDIATE, + HandBone::MiddleDistal => HandJoint::MIDDLE_DISTAL, + HandBone::MiddleTip => HandJoint::MIDDLE_TIP, + HandBone::RingMetacarpal => HandJoint::RING_METACARPAL, + HandBone::RingProximal => HandJoint::RING_PROXIMAL, + HandBone::RingIntermediate => HandJoint::RING_INTERMEDIATE, + HandBone::RingDistal => HandJoint::RING_DISTAL, + HandBone::RingTip => HandJoint::RING_TIP, + HandBone::LittleMetacarpal => HandJoint::LITTLE_METACARPAL, + HandBone::LittleProximal => HandJoint::LITTLE_PROXIMAL, + HandBone::LittleIntermediate => HandJoint::LITTLE_INTERMEDIATE, + HandBone::LittleDistal => HandJoint::LITTLE_DISTAL, + HandBone::LittleTip => HandJoint::LITTLE_TIP, + } +} + +fn get_bone_curl_angle(bone: HandJoint, curl: f32) -> f32 { + let mul: f32 = match bone { + HandJoint::INDEX_PROXIMAL => 0.0, + HandJoint::MIDDLE_PROXIMAL => 0.0, + HandJoint::RING_PROXIMAL => 0.0, + HandJoint::LITTLE_PROXIMAL => 0.0, + HandJoint::THUMB_PROXIMAL => 0.0, + _ => 1.0, + }; + let curl_angle = -((mul * curl * 80.0) + 5.0); + return curl_angle; +} + +fn log_hand(hand_pose: [Posef; 26]) { + let _palm_wrist = hand_pose[HandJoint::WRIST].position.to_vec3() + - hand_pose[HandJoint::PALM].position.to_vec3(); + info!( + "palm-wrist: {}", + hand_pose[HandJoint::WRIST].position.to_vec3() + - hand_pose[HandJoint::PALM].position.to_vec3() + ); + + info!( + "wrist-tm: {}", + hand_pose[HandJoint::THUMB_METACARPAL].position.to_vec3() + - hand_pose[HandJoint::WRIST].position.to_vec3() + ); + info!( + "tm-tp: {}", + hand_pose[HandJoint::THUMB_PROXIMAL].position.to_vec3() + - hand_pose[HandJoint::THUMB_METACARPAL].position.to_vec3() + ); + info!( + "tp-td: {}", + hand_pose[HandJoint::THUMB_DISTAL].position.to_vec3() + - hand_pose[HandJoint::THUMB_PROXIMAL].position.to_vec3() + ); + info!( + "td-tt: {}", + hand_pose[HandJoint::THUMB_TIP].position.to_vec3() + - hand_pose[HandJoint::THUMB_DISTAL].position.to_vec3() + ); + + info!( + "wrist-im: {}", + hand_pose[HandJoint::INDEX_METACARPAL].position.to_vec3() + - hand_pose[HandJoint::WRIST].position.to_vec3() + ); + info!( + "im-ip: {}", + hand_pose[HandJoint::INDEX_PROXIMAL].position.to_vec3() + - hand_pose[HandJoint::INDEX_METACARPAL].position.to_vec3() + ); + info!( + "ip-ii: {}", + hand_pose[HandJoint::INDEX_INTERMEDIATE].position.to_vec3() + - hand_pose[HandJoint::INDEX_PROXIMAL].position.to_vec3() + ); + info!( + "ii-id: {}", + hand_pose[HandJoint::INDEX_DISTAL].position.to_vec3() + - hand_pose[HandJoint::INDEX_INTERMEDIATE].position.to_vec3() + ); + info!( + "id-it: {}", + hand_pose[HandJoint::INDEX_TIP].position.to_vec3() + - hand_pose[HandJoint::INDEX_DISTAL].position.to_vec3() + ); + + info!( + "wrist-mm: {}", + hand_pose[HandJoint::MIDDLE_METACARPAL].position.to_vec3() + - hand_pose[HandJoint::WRIST].position.to_vec3() + ); + info!( + "mm-mp: {}", + hand_pose[HandJoint::MIDDLE_PROXIMAL].position.to_vec3() + - hand_pose[HandJoint::MIDDLE_METACARPAL].position.to_vec3() + ); + info!( + "mp-mi: {}", + hand_pose[HandJoint::MIDDLE_INTERMEDIATE].position.to_vec3() + - hand_pose[HandJoint::MIDDLE_PROXIMAL].position.to_vec3() + ); + info!( + "mi-md: {}", + hand_pose[HandJoint::MIDDLE_DISTAL].position.to_vec3() + - hand_pose[HandJoint::MIDDLE_INTERMEDIATE].position.to_vec3() + ); + info!( + "md-mt: {}", + hand_pose[HandJoint::MIDDLE_TIP].position.to_vec3() + - hand_pose[HandJoint::MIDDLE_DISTAL].position.to_vec3() + ); + + info!( + "wrist-rm: {}", + hand_pose[HandJoint::RING_METACARPAL].position.to_vec3() + - hand_pose[HandJoint::WRIST].position.to_vec3() + ); + info!( + "rm-rp: {}", + hand_pose[HandJoint::RING_PROXIMAL].position.to_vec3() + - hand_pose[HandJoint::RING_METACARPAL].position.to_vec3() + ); + info!( + "rp-ri: {}", + hand_pose[HandJoint::RING_INTERMEDIATE].position.to_vec3() + - hand_pose[HandJoint::RING_PROXIMAL].position.to_vec3() + ); + info!( + "ri-rd: {}", + hand_pose[HandJoint::RING_DISTAL].position.to_vec3() + - hand_pose[HandJoint::RING_INTERMEDIATE].position.to_vec3() + ); + info!( + "rd-rt: {}", + hand_pose[HandJoint::RING_TIP].position.to_vec3() + - hand_pose[HandJoint::RING_DISTAL].position.to_vec3() + ); + + info!( + "wrist-lm: {}", + hand_pose[HandJoint::LITTLE_METACARPAL].position.to_vec3() + - hand_pose[HandJoint::WRIST].position.to_vec3() + ); + info!( + "lm-lp: {}", + hand_pose[HandJoint::LITTLE_PROXIMAL].position.to_vec3() + - hand_pose[HandJoint::LITTLE_METACARPAL].position.to_vec3() + ); + info!( + "lp-li: {}", + hand_pose[HandJoint::LITTLE_INTERMEDIATE].position.to_vec3() + - hand_pose[HandJoint::LITTLE_PROXIMAL].position.to_vec3() + ); + info!( + "li-ld: {}", + hand_pose[HandJoint::LITTLE_DISTAL].position.to_vec3() + - hand_pose[HandJoint::LITTLE_INTERMEDIATE].position.to_vec3() + ); + info!( + "ld-lt: {}", + hand_pose[HandJoint::LITTLE_TIP].position.to_vec3() + - hand_pose[HandJoint::LITTLE_DISTAL].position.to_vec3() + ); +} + +pub fn update_hand_skeletons( + right_controller_query: Query<(&GlobalTransform, With)>, + left_controller_query: Query<(&GlobalTransform, With)>, + hand_states_option: Option>, + mut hand_bone_query: Query<(&mut Transform, &HandBone, &Hand)>, + input_source: Option>, +) { + match input_source { + Some(res) => match *res { + HandInputSource::Emulated => { + // info!("hand input source is emulated"); + match hand_states_option { + Some(hands) => { + let left_hand_transform = left_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + update_hand_bones_emulated( + left_hand_transform, + Hand::Left, + hands.left, + &mut hand_bone_query, + ); + let right_hand_transform = right_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + update_hand_bones_emulated( + right_hand_transform, + Hand::Right, + hands.right, + &mut hand_bone_query, + ); + } + None => info!("hand states resource not initialized yet"), + } + } + HandInputSource::OpenXr => { + info!("hand input source is open XR: this is not implemented yet"); + return; + } + }, + None => { + info!("hand input source not initialized"); + return; + } + } +} + +pub fn draw_hand_entities(mut gizmos: Gizmos, query: Query<(&Transform, &HandBone)>) { + for (transform, hand_bone) in query.iter() { + let (radius, color) = get_bone_gizmo_style(hand_bone); + gizmos.sphere(transform.translation, transform.rotation, radius, color); + } +} + +fn get_bone_gizmo_style(hand_bone: &HandBone) -> (f32, Color) { + match hand_bone { + HandBone::Palm => (0.01, Color::WHITE), + HandBone::Wrist => (0.01, Color::GRAY), + HandBone::ThumbMetacarpal => (0.01, Color::RED), + HandBone::ThumbProximal => (0.008, Color::RED), + HandBone::ThumbDistal => (0.006, Color::RED), + HandBone::ThumbTip => (0.004, Color::RED), + HandBone::IndexMetacarpal => (0.01, Color::ORANGE), + HandBone::IndexProximal => (0.008, Color::ORANGE), + HandBone::IndexIntermediate => (0.006, Color::ORANGE), + HandBone::IndexDistal => (0.004, Color::ORANGE), + HandBone::IndexTip => (0.002, Color::ORANGE), + HandBone::MiddleMetacarpal => (0.01, Color::YELLOW), + HandBone::MiddleProximal => (0.008, Color::YELLOW), + HandBone::MiddleIntermediate => (0.006, Color::YELLOW), + HandBone::MiddleDistal => (0.004, Color::YELLOW), + HandBone::MiddleTip => (0.002, Color::YELLOW), + HandBone::RingMetacarpal => (0.01, Color::GREEN), + HandBone::RingProximal => (0.008, Color::GREEN), + HandBone::RingIntermediate => (0.006, Color::GREEN), + HandBone::RingDistal => (0.004, Color::GREEN), + HandBone::RingTip => (0.002, Color::GREEN), + HandBone::LittleMetacarpal => (0.01, Color::BLUE), + HandBone::LittleProximal => (0.008, Color::BLUE), + HandBone::LittleIntermediate => (0.006, Color::BLUE), + HandBone::LittleDistal => (0.004, Color::BLUE), + HandBone::LittleTip => (0.002, Color::BLUE), + } +} diff --git a/src/xr_input/hand_poses.rs b/src/xr_input/hand_poses.rs new file mode 100644 index 0000000..70544eb --- /dev/null +++ b/src/xr_input/hand_poses.rs @@ -0,0 +1,520 @@ +use bevy::prelude::{Quat, Transform, Vec3}; +use openxr::{Posef, Quaternionf, Vector3f}; + +use super::Hand; + +pub fn get_simulated_open_hand_transforms(hand: Hand) -> [Transform; 26] { + let test_hand_bones: [Vec3; 26] = [ + Vec3 { + x: 0.0, + y: 0.0, + z: 0.0, + }, //palm + Vec3 { + x: 0.0, + y: 0.0, + z: -0.04, + }, //wrist + Vec3 { + x: -0.02, + y: 0.00, + z: 0.015, + }, //thumb + Vec3 { + x: 0.0, + y: 0.0, + z: 0.03, + }, + Vec3 { + x: 0.0, + y: 0.0, + z: 0.024, + }, + Vec3 { + x: 0.0, + y: 0.0, + z: 0.024, + }, + Vec3 { + x: -0.01, + y: -0.015, + z: 0.0155, + }, //index + Vec3 { + x: 0.0, + y: 0.0, + z: 0.064, + }, + Vec3 { + x: 0.0, + y: 0.0, + z: 0.037, + }, + Vec3 { + x: 0.0, + y: 0.0, + z: 0.02, + }, + Vec3 { + x: 0.0, + y: 0.0, + z: 0.01, + }, + Vec3 { + x: 0.0, + y: -0.02, + z: 0.016, + }, //middle + Vec3 { + x: 0.0, + y: 0.0, + z: 0.064, + }, + Vec3 { + x: 0.0, + y: 0.0, + z: 0.037, + }, + Vec3 { + x: 0.0, + y: 0.0, + z: 0.02, + }, + Vec3 { + x: 0.0, + y: 0.0, + z: 0.01, + }, + Vec3 { + x: 0.01, + y: -0.015, + z: 0.015, + }, //ring + Vec3 { + x: 0.0, + y: 0.0, + z: 0.064, + }, + Vec3 { + x: 0.0, + y: 0.0, + z: 0.037, + }, + Vec3 { + x: 0.0, + y: 0.0, + z: 0.02, + }, + Vec3 { + x: 0.0, + y: 0.0, + z: 0.01, + }, + Vec3 { + x: 0.02, + y: -0.01, + z: 0.015, + }, //little + Vec3 { + x: 0.0, + y: 0.0, + z: 0.064, + }, + Vec3 { + x: 0.0, + y: 0.0, + z: 0.037, + }, + Vec3 { + x: 0.0, + y: 0.0, + z: 0.02, + }, + Vec3 { + x: 0.0, + y: 0.0, + z: 0.01, + }, + ]; + let result = bones_to_transforms(test_hand_bones, hand); + return result; +} + +fn bones_to_transforms(hand_bones: [Vec3; 26], hand: Hand) -> [Transform; 26] { + match hand { + Hand::Left => { + let mut result_array: [Transform; 26] = [Transform::default(); 26]; + for (place, data) in result_array.iter_mut().zip(hand_bones.iter()) { + *place = Transform { + translation: Vec3 { + x: -data.x, + y: -data.y, + z: -data.z, + }, + rotation: Quat::IDENTITY, + scale: Vec3::splat(1.0), + } + } + return result_array; + } + Hand::Right => { + let mut result_array: [Transform; 26] = [Transform::default(); 26]; + for (place, data) in result_array.iter_mut().zip(hand_bones.iter()) { + *place = Transform { + translation: Vec3 { + x: data.x, + y: -data.y, + z: -data.z, + }, + rotation: Quat::IDENTITY, + scale: Vec3::splat(1.0), + } + } + return result_array; + } + } +} + +pub fn get_test_hand_pose_array() -> [Posef; 26] { + let test_hand_pose: [Posef; 26] = [ + Posef { + position: Vector3f { + x: 0.0, + y: 0.0, + z: 0.0, + }, + orientation: Quaternionf { + x: -0.267, + y: 0.849, + z: 0.204, + w: 0.407, + }, + }, //palm + Posef { + position: Vector3f { + x: 0.02, + y: -0.040, + z: -0.015, + }, + orientation: Quaternionf { + x: -0.267, + y: 0.849, + z: 0.204, + w: 0.407, + }, + }, + Posef { + position: Vector3f { + x: 0.019, + y: -0.037, + z: 0.011, + }, + orientation: Quaternionf { + x: -0.744, + y: -0.530, + z: 0.156, + w: -0.376, + }, + }, + Posef { + position: Vector3f { + x: 0.015, + y: -0.014, + z: 0.047, + }, + orientation: Quaternionf { + x: -0.786, + y: -0.550, + z: 0.126, + w: -0.254, + }, + }, + Posef { + position: Vector3f { + x: 0.004, + y: 0.003, + z: 0.068, + }, + orientation: Quaternionf { + x: -0.729, + y: -0.564, + z: 0.027, + w: -0.387, + }, + }, + Posef { + position: Vector3f { + x: -0.009, + y: 0.011, + z: 0.072, + }, + orientation: Quaternionf { + x: -0.585, + y: -0.548, + z: -0.140, + w: -0.582, + }, + }, + Posef { + position: Vector3f { + x: 0.027, + y: -0.021, + z: 0.001, + }, + orientation: Quaternionf { + x: -0.277, + y: -0.826, + z: 0.317, + w: -0.376, + }, + }, + Posef { + position: Vector3f { + x: -0.002, + y: 0.026, + z: 0.034, + }, + orientation: Quaternionf { + x: -0.277, + y: -0.826, + z: 0.317, + w: -0.376, + }, + }, + Posef { + position: Vector3f { + x: -0.023, + y: 0.049, + z: 0.055, + }, + orientation: Quaternionf { + x: -0.244, + y: -0.843, + z: 0.256, + w: -0.404, + }, + }, + Posef { + position: Vector3f { + x: -0.037, + y: 0.059, + z: 0.067, + }, + orientation: Quaternionf { + x: -0.200, + y: -0.866, + z: 0.165, + w: -0.428, + }, + }, + Posef { + position: Vector3f { + x: -0.045, + y: 0.063, + z: 0.073, + }, + orientation: Quaternionf { + x: -0.172, + y: -0.874, + z: 0.110, + w: -0.440, + }, + }, + Posef { + position: Vector3f { + x: 0.021, + y: -0.017, + z: -0.007, + }, + orientation: Quaternionf { + x: -0.185, + y: -0.817, + z: 0.370, + w: -0.401, + }, + }, + Posef { + position: Vector3f { + x: -0.011, + y: 0.029, + z: 0.018, + }, + orientation: Quaternionf { + x: -0.185, + y: -0.817, + z: 0.370, + w: -0.401, + }, + }, + Posef { + position: Vector3f { + x: -0.034, + y: 0.06, + z: 0.033, + }, + orientation: Quaternionf { + x: -0.175, + y: -0.809, + z: 0.371, + w: -0.420, + }, + }, + Posef { + position: Vector3f { + x: -0.051, + y: 0.072, + z: 0.045, + }, + orientation: Quaternionf { + x: -0.109, + y: -0.856, + z: 0.245, + w: -0.443, + }, + }, + Posef { + position: Vector3f { + x: -0.06, + y: 0.077, + z: 0.051, + }, + orientation: Quaternionf { + x: -0.075, + y: -0.871, + z: 0.180, + w: -0.450, + }, + }, + Posef { + position: Vector3f { + x: 0.013, + y: -0.017, + z: -0.015, + }, + orientation: Quaternionf { + x: -0.132, + y: -0.786, + z: 0.408, + w: -0.445, + }, + }, + Posef { + position: Vector3f { + x: -0.02, + y: 0.025, + z: 0.0, + }, + orientation: Quaternionf { + x: -0.132, + y: -0.786, + z: 0.408, + w: -0.445, + }, + }, + Posef { + position: Vector3f { + x: -0.042, + y: 0.055, + z: 0.007, + }, + orientation: Quaternionf { + x: -0.131, + y: -0.762, + z: 0.432, + w: -0.464, + }, + }, + Posef { + position: Vector3f { + x: -0.06, + y: 0.069, + z: 0.015, + }, + orientation: Quaternionf { + x: -0.071, + y: -0.810, + z: 0.332, + w: -0.477, + }, + }, + Posef { + position: Vector3f { + x: -0.069, + y: 0.075, + z: 0.02, + }, + orientation: Quaternionf { + x: -0.029, + y: -0.836, + z: 0.260, + w: -0.482, + }, + }, + Posef { + position: Vector3f { + x: 0.004, + y: -0.022, + z: -0.022, + }, + orientation: Quaternionf { + x: -0.060, + y: -0.749, + z: 0.481, + w: -0.452, + }, + }, + Posef { + position: Vector3f { + x: -0.028, + y: 0.018, + z: -0.015, + }, + orientation: Quaternionf { + x: -0.060, + y: -0.749, + z: 0.481, + w: -0.452, + }, + }, + Posef { + position: Vector3f { + x: -0.046, + y: 0.042, + z: -0.017, + }, + orientation: Quaternionf { + x: -0.061, + y: -0.684, + z: 0.534, + w: -0.493, + }, + }, + Posef { + position: Vector3f { + x: -0.059, + y: 0.053, + z: -0.015, + }, + orientation: Quaternionf { + x: 0.002, + y: -0.745, + z: 0.444, + w: -0.498, + }, + }, + Posef { + position: Vector3f { + x: -0.068, + y: 0.059, + z: -0.013, + }, + orientation: Quaternionf { + x: 0.045, + y: -0.780, + z: 0.378, + w: -0.496, + }, + }, + ]; + return test_hand_pose; +} diff --git a/src/xr_input/interactions.rs b/src/xr_input/interactions.rs new file mode 100644 index 0000000..9660839 --- /dev/null +++ b/src/xr_input/interactions.rs @@ -0,0 +1,370 @@ +use std::f32::consts::PI; + +use bevy::prelude::{ + info, Color, Component, Entity, Event, EventReader, EventWriter, Gizmos, GlobalTransform, Quat, + Query, Transform, Vec3, With, Without, +}; + +use super::trackers::{AimPose, OpenXRTrackingRoot}; + +#[derive(Component)] +pub struct XRDirectInteractor; + +#[derive(Component)] +pub struct XRRayInteractor; + +#[derive(Component)] +pub struct XRSocketInteractor; + +#[derive(Component)] +pub struct Touched(pub bool); + +#[derive(Component, Clone, Copy, PartialEq, PartialOrd, Debug)] +pub enum XRInteractableState { + Idle, + Hover, + Select, +} + +impl Default for XRInteractableState { + fn default() -> Self { + XRInteractableState::Idle + } +} + +#[derive(Component)] +pub enum XRInteractorState { + Idle, + Selecting, +} +impl Default for XRInteractorState { + fn default() -> Self { + XRInteractorState::Idle + } +} + +#[derive(Component)] +pub struct XRInteractable; + +pub fn draw_socket_gizmos( + mut gizmos: Gizmos, + interactor_query: Query<( + &GlobalTransform, + &XRInteractorState, + Entity, + &XRSocketInteractor, + )>, +) { + for (global, state, _entity, _socket) in interactor_query.iter() { + let mut transform = global.compute_transform().clone(); + transform.scale = Vec3::splat(0.1); + let color = match state { + XRInteractorState::Idle => Color::BLUE, + XRInteractorState::Selecting => Color::PURPLE, + }; + gizmos.cuboid(transform, color) + } +} + +pub fn draw_interaction_gizmos( + mut gizmos: Gizmos, + interactable_query: Query< + (&GlobalTransform, &XRInteractableState), + (With, Without), + >, + interactor_query: Query< + ( + &GlobalTransform, + &XRInteractorState, + Option<&XRDirectInteractor>, + Option<&XRRayInteractor>, + Option<&AimPose>, + ), + Without, + >, + tracking_root_query: Query<(&mut Transform, With)>, +) { + let root = tracking_root_query.get_single().unwrap().0; + for (global_transform, interactable_state) in interactable_query.iter() { + let transform = global_transform.compute_transform(); + let color = match interactable_state { + XRInteractableState::Idle => Color::RED, + XRInteractableState::Hover => Color::YELLOW, + XRInteractableState::Select => Color::GREEN, + }; + gizmos.sphere(transform.translation, transform.rotation, 0.1, color); + } + + for (interactor_global_transform, interactor_state, direct, ray, aim) in interactor_query.iter() + { + let transform = interactor_global_transform.compute_transform(); + match direct { + Some(_) => { + let mut local = transform.clone(); + local.scale = Vec3::splat(0.1); + let quat = Quat::from_euler( + bevy::prelude::EulerRot::XYZ, + 45.0 * (PI / 180.0), + 0.0, + 45.0 * (PI / 180.0), + ); + local.rotation = quat; + let color = match interactor_state { + XRInteractorState::Idle => Color::BLUE, + XRInteractorState::Selecting => Color::PURPLE, + }; + gizmos.cuboid(local, color); + } + None => (), + } + match ray { + Some(_) => match aim { + Some(aim) => { + let color = match interactor_state { + XRInteractorState::Idle => Color::BLUE, + XRInteractorState::Selecting => Color::PURPLE, + }; + gizmos.ray( + root.translation + root.rotation.mul_vec3(aim.0.translation), + root.rotation.mul_vec3(aim.0.forward()), + color, + ); + } + None => todo!(), + }, + None => (), + } + } +} + +#[derive(Event)] +pub struct InteractionEvent { + pub interactor: Entity, + pub interactable: Entity, + pub interactable_state: XRInteractableState, +} + +pub fn socket_interactions( + interactable_query: Query< + (&GlobalTransform, &mut XRInteractableState, Entity), + (With, Without), + >, + interactor_query: Query< + ( + &GlobalTransform, + &XRInteractorState, + Entity, + &XRSocketInteractor, + ), + Without, + >, + mut writer: EventWriter, +) { + for interactable in interactable_query.iter() { + //for the interactables + for socket in interactor_query.iter() { + let interactor_global_transform = socket.0; + let xr_interactable_global_transform = interactable.0; + let interactor_state = socket.1; + //check for sphere overlaps + let size = 0.1; + if interactor_global_transform + .compute_transform() + .translation + .distance_squared( + xr_interactable_global_transform + .compute_transform() + .translation, + ) + < (size * size) * 2.0 + { + //check for selections first + match interactor_state { + XRInteractorState::Idle => { + let event = InteractionEvent { + interactor: socket.2, + interactable: interactable.2, + interactable_state: XRInteractableState::Hover, + }; + writer.send(event); + } + XRInteractorState::Selecting => { + let event = InteractionEvent { + interactor: socket.2, + interactable: interactable.2, + interactable_state: XRInteractableState::Select, + }; + writer.send(event); + } + } + } + } + } +} + +pub fn interactions( + interactable_query: Query< + (&GlobalTransform, Entity), + (With, Without), + >, + interactor_query: Query< + ( + &GlobalTransform, + &XRInteractorState, + Entity, + Option<&XRDirectInteractor>, + Option<&XRRayInteractor>, + Option<&AimPose>, + ), + Without, + >, + tracking_root_query: Query<(&mut Transform, With)>, + mut writer: EventWriter, +) { + for (xr_interactable_global_transform, interactable_entity) in interactable_query.iter() { + for (interactor_global_transform, interactor_state, interactor_entity, direct, ray, aim) in + interactor_query.iter() + { + match direct { + Some(_) => { + //check for sphere overlaps + let size = 0.1; + if interactor_global_transform + .compute_transform() + .translation + .distance_squared( + xr_interactable_global_transform + .compute_transform() + .translation, + ) + < (size * size) * 2.0 + { + //check for selections first + match interactor_state { + XRInteractorState::Idle => { + let event = InteractionEvent { + interactor: interactor_entity, + interactable: interactable_entity, + interactable_state: XRInteractableState::Hover, + }; + writer.send(event); + } + XRInteractorState::Selecting => { + let event = InteractionEvent { + interactor: interactor_entity, + interactable: interactable_entity, + interactable_state: XRInteractableState::Select, + }; + writer.send(event); + } + } + } + } + None => (), + } + match ray { + Some(_) => { + //check for ray-sphere intersection + let sphere_transform = xr_interactable_global_transform.compute_transform(); + let center = sphere_transform.translation; + let radius: f32 = 0.1; + //I hate this but the aim pose needs the root for now + let root = tracking_root_query.get_single().unwrap().0; + match aim { + Some(aim) => { + let ray_origin = + root.translation + root.rotation.mul_vec3(aim.0.translation); + let ray_dir = root.rotation.mul_vec3(aim.0.forward()); + + if ray_sphere_intersection( + center, + radius, + ray_origin, + ray_dir.normalize_or_zero(), + ) { + //check for selections first + match interactor_state { + XRInteractorState::Idle => { + let event = InteractionEvent { + interactor: interactor_entity, + interactable: interactable_entity, + interactable_state: XRInteractableState::Hover, + }; + writer.send(event); + } + XRInteractorState::Selecting => { + let event = InteractionEvent { + interactor: interactor_entity, + interactable: interactable_entity, + interactable_state: XRInteractableState::Select, + }; + writer.send(event); + } + } + } + } + None => info!("no aim pose"), + } + } + None => (), + } + } + } +} + +pub fn update_interactable_states( + mut events: EventReader, + mut interactable_query: Query< + (Entity, &mut XRInteractableState, &mut Touched), + With, + >, +) { + //i very much dislike this + for (_entity, _state, mut touched) in interactable_query.iter_mut() { + *touched = Touched(false); + } + for event in events.read() { + //lets change the state + match interactable_query.get_mut(event.interactable) { + Ok((_entity, mut entity_state, mut touched)) => { + //since we have an event we were touched this frame, i hate this name + *touched = Touched(true); + if event.interactable_state > *entity_state { + // info!( + // "event.state: {:?}, interactable.state: {:?}", + // event.interactable_state, entity_state + // ); + // info!("event has a higher state"); + } + *entity_state = event.interactable_state; + } + Err(_) => {} + } + } + //lets go through all the untouched interactables and set them to idle + for (_entity, mut state, touched) in interactable_query.iter_mut() { + if !touched.0 { + *state = XRInteractableState::Idle; + } + } +} + +fn ray_sphere_intersection(center: Vec3, radius: f32, ray_origin: Vec3, ray_dir: Vec3) -> bool { + let l = center - ray_origin; + let adj = l.dot(ray_dir); + let d2 = l.dot(l) - (adj * adj); + let radius2 = radius * radius; + if d2 > radius2 { + return false; + } + let thc = (radius2 - d2).sqrt(); + let t0 = adj - thc; + let t1 = adj + thc; + + if t0 < 0.0 && t1 < 0.0 { + return false; + } + + // let distance = if t0 < t1 { t0 } else { t1 }; + return true; +} diff --git a/src/xr_input/mod.rs b/src/xr_input/mod.rs index 55e7c56..8746a17 100644 --- a/src/xr_input/mod.rs +++ b/src/xr_input/mod.rs @@ -1,9 +1,12 @@ pub mod controllers; pub mod debug_gizmos; +pub mod interactions; pub mod oculus_touch; pub mod prototype_locomotion; pub mod trackers; pub mod xr_camera; +pub mod hand_poses; +pub mod hand; use crate::resources::XrSession; use crate::xr_begin_frame; @@ -12,7 +15,7 @@ use crate::xr_input::oculus_touch::{setup_oculus_controller, ActionSets}; use crate::xr_input::xr_camera::{xr_camera_head_sync, Eye, XRProjection, XrCameraBundle}; use bevy::app::{App, PostUpdate, Startup}; use bevy::log::warn; -use bevy::prelude::{BuildChildren, IntoSystemConfigs}; +use bevy::prelude::{BuildChildren, IntoSystemConfigs, Component}; use bevy::prelude::{Commands, Plugin, PreUpdate, Quat, Res, SpatialBundle, Update, Vec3}; use bevy::render::camera::CameraProjectionPlugin; use bevy::render::view::{update_frusta, VisibilitySystems}; @@ -27,7 +30,7 @@ use self::trackers::{ pub struct OpenXrInput { pub controller_type: XrControllerType, } -#[derive(Clone, Copy, Debug, Ord, PartialOrd, Eq, PartialEq)] +#[derive(Clone, Copy, Debug, Ord, PartialOrd, Eq, PartialEq, Component)] pub enum Hand { Left, Right, diff --git a/src/xr_input/prototype_locomotion.rs b/src/xr_input/prototype_locomotion.rs index 615f9fd..71e708f 100644 --- a/src/xr_input/prototype_locomotion.rs +++ b/src/xr_input/prototype_locomotion.rs @@ -43,7 +43,7 @@ pub struct PrototypeLocomotionConfig { impl Default for PrototypeLocomotionConfig { fn default() -> Self { Self { - locomotion_type: LocomotionType::Hand, + locomotion_type: LocomotionType::Head, locomotion_speed: 1.0, rotation_type: RotationType::Smooth, snap_angle: 45.0 * (PI / 180.0), diff --git a/src/xr_input/trackers.rs b/src/xr_input/trackers.rs index cbf79dd..47c063e 100644 --- a/src/xr_input/trackers.rs +++ b/src/xr_input/trackers.rs @@ -1,8 +1,14 @@ -use bevy::prelude::{Added, BuildChildren, Commands, Entity, Query, With, Res, Transform, Without, Component, info}; +use bevy::prelude::{ + info, Added, BuildChildren, Commands, Component, Entity, Query, Res, Transform, Vec3, With, + Without, +}; -use crate::{resources::{XrFrameState, XrInstance, XrSession}, input::XrInput}; +use crate::{ + input::XrInput, + resources::{XrFrameState, XrInstance, XrSession}, +}; -use super::{oculus_touch::OculusController, Hand, Vec3Conv, QuatConv}; +use super::{oculus_touch::OculusController, Hand, QuatConv, Vec3Conv}; #[derive(Component)] pub struct OpenXRTrackingRoot; @@ -20,6 +26,8 @@ pub struct OpenXRLeftController; pub struct OpenXRRightController; #[derive(Component)] pub struct OpenXRController; +#[derive(Component)] +pub struct AimPose(pub Transform); pub fn adopt_open_xr_trackers( query: Query>, @@ -43,11 +51,13 @@ pub fn update_open_xr_controllers( oculus_controller: Res, mut left_controller_query: Query<( &mut Transform, + Option<&mut AimPose>, With, Without, )>, mut right_controller_query: Query<( &mut Transform, + Option<&mut AimPose>, With, Without, )>, @@ -61,8 +71,20 @@ pub fn update_open_xr_controllers( //get controller let controller = oculus_controller.get_ref(&instance, &session, &frame_state, &xr_input); //get left controller - let left = controller.grip_space(Hand::Left); - let left_postion = left.0.pose.position.to_vec3(); + let left_grip_space = controller.grip_space(Hand::Left); + let left_aim_space = controller.aim_space(Hand::Left); + let left_postion = left_grip_space.0.pose.position.to_vec3(); + let left_aim_pose = left_controller_query.get_single_mut().unwrap().1; + match left_aim_pose { + Some(mut pose) => { + *pose = AimPose(Transform { + translation: left_aim_space.0.pose.position.to_vec3(), + rotation: left_aim_space.0.pose.orientation.to_quat(), + scale: Vec3::splat(1.0), + }); + } + None => (), + } left_controller_query .get_single_mut() @@ -70,10 +92,24 @@ pub fn update_open_xr_controllers( .0 .translation = left_postion; - left_controller_query.get_single_mut().unwrap().0.rotation = left.0.pose.orientation.to_quat(); + left_controller_query.get_single_mut().unwrap().0.rotation = + left_grip_space.0.pose.orientation.to_quat(); //get right controller - let right = controller.grip_space(Hand::Right); - let right_postion = right.0.pose.position.to_vec3(); + let right_grip_space = controller.grip_space(Hand::Right); + let right_aim_space = controller.aim_space(Hand::Right); + let right_postion = right_grip_space.0.pose.position.to_vec3(); + + let right_aim_pose = right_controller_query.get_single_mut().unwrap().1; + match right_aim_pose { + Some(mut pose) => { + *pose = AimPose(Transform { + translation: right_aim_space.0.pose.position.to_vec3(), + rotation: right_aim_space.0.pose.orientation.to_quat(), + scale: Vec3::splat(1.0), + }); + } + None => (), + } right_controller_query .get_single_mut() @@ -82,6 +118,5 @@ pub fn update_open_xr_controllers( .translation = right_postion; right_controller_query.get_single_mut().unwrap().0.rotation = - right.0.pose.orientation.to_quat(); + right_grip_space.0.pose.orientation.to_quat(); } -