From 6b03a59b77c663661c727a2becb2cefa2d822f39 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Thu, 21 Sep 2023 13:14:59 -0400 Subject: [PATCH 01/66] pinned to working version for now --- Cargo.toml | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 84e1f11..58c5101 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -10,7 +10,9 @@ linked = ["openxr/linked", "openxr/static"] [dependencies] anyhow = "1.0.75" ash = "0.37.3" -bevy = { git = "https://github.com/awtterpip/bevy", default-features = false, features = ["bevy_render"] } +bevy = { git = "https://github.com/awtterpip/bevy", default-features = false, features = [ + "bevy_render", +], rev = "ac28b11797c0a85b431ee4940c6afa434f712f7a" } openxr = { version = "0.17.1", features = ["mint"] } mint = "0.5.9" wgpu = "0.16.0" @@ -18,7 +20,7 @@ wgpu-core = { version = "0.16.0", features = ["vulkan"] } wgpu-hal = "0.16.0" [dev-dependencies] -bevy = { git = "https://github.com/awtterpip/bevy" } +bevy = { git = "https://github.com/awtterpip/bevy", rev = "ac28b11797c0a85b431ee4940c6afa434f712f7a" } color-eyre = "0.6.2" [[example]] @@ -26,4 +28,4 @@ name = "xr" path = "examples/xr.rs" [profile.release] -debug = true \ No newline at end of file +debug = true From 23ed65f06a5bb35ba115c1604ffd64f57b609dc9 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sun, 24 Sep 2023 23:15:24 -0400 Subject: [PATCH 02/66] added initial interactions --- Cargo.toml | 4 +- examples/xr.rs | 69 ++++++++++++++++++++ src/xr_input/interactions.rs | 123 +++++++++++++++++++++++++++++++++++ src/xr_input/mod.rs | 1 + 4 files changed, 195 insertions(+), 2 deletions(-) create mode 100644 src/xr_input/interactions.rs diff --git a/Cargo.toml b/Cargo.toml index 58c5101..578d584 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -12,7 +12,7 @@ anyhow = "1.0.75" ash = "0.37.3" bevy = { git = "https://github.com/awtterpip/bevy", default-features = false, features = [ "bevy_render", -], rev = "ac28b11797c0a85b431ee4940c6afa434f712f7a" } +] } openxr = { version = "0.17.1", features = ["mint"] } mint = "0.5.9" wgpu = "0.16.0" @@ -20,7 +20,7 @@ wgpu-core = { version = "0.16.0", features = ["vulkan"] } wgpu-hal = "0.16.0" [dev-dependencies] -bevy = { git = "https://github.com/awtterpip/bevy", rev = "ac28b11797c0a85b431ee4940c6afa434f712f7a" } +bevy = { git = "https://github.com/awtterpip/bevy" } color-eyre = "0.6.2" [[example]] diff --git a/examples/xr.rs b/examples/xr.rs index 548fe15..36e835c 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,11 +1,19 @@ use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; +use bevy_openxr::input::XrInput; +use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; +use bevy_openxr::xr_input::interactions::{ + draw_interaction_gizmos, hover_interaction, XRDirectInteractor, XRInteractable, + XRInteractableState, XRInteractorState, +}; +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, }; +use bevy_openxr::xr_input::Hand; use bevy_openxr::DefaultXrPlugins; fn main() { @@ -21,6 +29,9 @@ fn main() { .add_systems(Update, proto_locomotion) .add_systems(Startup, spawn_controllers_example) .insert_resource(PrototypeLocomotionConfig::default()) + .add_systems(Update, draw_interaction_gizmos) + .add_systems(Update, hover_interaction) + .add_systems(Update, prototype_interaction_input) .run(); } @@ -65,6 +76,15 @@ fn setup( transform: Transform::from_xyz(-2.0, 2.5, 5.0).looking_at(Vec3::ZERO, Vec3::Y), ..default() },)); + //simple interactable + commands.spawn(( + SpatialBundle { + transform: Transform::from_xyz(0.0, 1.0, 0.0), + ..default() + }, + XRInteractable, + XRInteractableState::default(), + )); } fn spawn_controllers_example(mut commands: Commands) { @@ -74,6 +94,8 @@ fn spawn_controllers_example(mut commands: Commands) { OpenXRController, OpenXRTracker, SpatialBundle::default(), + XRDirectInteractor, + XRInteractorState::default(), )); //right hand commands.spawn(( @@ -81,5 +103,52 @@ 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; + } +} diff --git a/src/xr_input/interactions.rs b/src/xr_input/interactions.rs new file mode 100644 index 0000000..ea34112 --- /dev/null +++ b/src/xr_input/interactions.rs @@ -0,0 +1,123 @@ +use std::f32::consts::PI; + +use bevy::prelude::{ + info, Color, Component, Gizmos, GlobalTransform, Quat, Query, Vec3, With, Without, +}; + +#[derive(Component)] +pub struct XRDirectInteractor; +#[derive(Component)] +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_interaction_gizmos( + mut gizmos: Gizmos, + interactable_query: Query< + (&GlobalTransform, &XRInteractableState), + (With, Without), + >, + interactor_query: Query< + (&GlobalTransform, &XRInteractorState), + (With, Without), + >, +) { + 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) in interactor_query.iter() { + let mut transform = interactor_global_transform.compute_transform(); + transform.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), + ); + transform.rotation = quat; + let color = match interactor_state { + XRInteractorState::Idle => Color::BLUE, + XRInteractorState::Selecting => Color::PURPLE, + }; + gizmos.cuboid(transform, color); + } +} + +pub fn hover_interaction( + mut interactable_query: Query< + (&GlobalTransform, &mut XRInteractableState), + (With, Without), + >, + interactor_query: Query< + (&GlobalTransform, &XRInteractorState), + (With, Without), + >, +) { + 'interactable: for (xr_interactable_global_transform, mut state) in + interactable_query.iter_mut() + { + let mut hovered = false; + let mut selected = false; + for (interactor_global_transform, interactor_state) in interactor_query.iter() { + //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 + { + info!("we overlapping"); + //check for selections first + match interactor_state { + XRInteractorState::Idle => hovered = true, + XRInteractorState::Selecting => { + selected = true; + } + } + } + } + //check what we found + //also i dont like this + if selected { + *state = XRInteractableState::Select; + } else if hovered { + *state = XRInteractableState::Hover; + } else { + *state = XRInteractableState::Idle; + } + } +} diff --git a/src/xr_input/mod.rs b/src/xr_input/mod.rs index 55e7c56..fe77373 100644 --- a/src/xr_input/mod.rs +++ b/src/xr_input/mod.rs @@ -1,5 +1,6 @@ pub mod controllers; pub mod debug_gizmos; +pub mod interactions; pub mod oculus_touch; pub mod prototype_locomotion; pub mod trackers; From 328c574f9e96d3ff439b0d56886190af6ea63b00 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sun, 24 Sep 2023 23:30:50 -0400 Subject: [PATCH 03/66] removed worthless logging, and loop tag --- src/xr_input/interactions.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/xr_input/interactions.rs b/src/xr_input/interactions.rs index ea34112..2fa5c35 100644 --- a/src/xr_input/interactions.rs +++ b/src/xr_input/interactions.rs @@ -82,7 +82,7 @@ pub fn hover_interaction( (With, Without), >, ) { - 'interactable: for (xr_interactable_global_transform, mut state) in + for (xr_interactable_global_transform, mut state) in interactable_query.iter_mut() { let mut hovered = false; @@ -100,7 +100,7 @@ pub fn hover_interaction( ) < (size * size) * 2.0 { - info!("we overlapping"); + //check for selections first match interactor_state { XRInteractorState::Idle => hovered = true, From 16bbe283cbccb92c73fd66175c2602c9a3fd80a2 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sun, 24 Sep 2023 23:46:57 -0400 Subject: [PATCH 04/66] change default locomotion to head tracked --- src/xr_input/prototype_locomotion.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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), From 4b1536cb0b29ae6a176acdaf2c5a3141f285d1a3 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 26 Sep 2023 12:54:52 -0400 Subject: [PATCH 05/66] got a hand array --- examples/xr.rs | 105 ++++++++++++++++++++++++++++++++++++++++++- src/xr_input/hand.rs | 3 ++ 2 files changed, 107 insertions(+), 1 deletion(-) create mode 100644 src/xr_input/hand.rs diff --git a/examples/xr.rs b/examples/xr.rs index 548fe15..80eb3b7 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,12 +1,14 @@ use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; +use bevy_openxr::xr_input::{Vec3Conv, QuatConv}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, }; use bevy_openxr::DefaultXrPlugins; +use openxr::{Posef, Quaternionf, Vector3f, HandJoint}; fn main() { color_eyre::install().unwrap(); @@ -20,6 +22,7 @@ fn main() { .add_systems(Startup, setup) .add_systems(Update, proto_locomotion) .add_systems(Startup, spawn_controllers_example) + .add_systems(Update, spawn_skeleton) .insert_resource(PrototypeLocomotionConfig::default()) .run(); } @@ -62,11 +65,111 @@ 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() },)); } +fn spawn_skeleton(mut commands: Commands, mut gizmos: Gizmos) { + let hand_pose: [Posef; 26] = [ + Posef { position: Vector3f {x: -0.548, y: -0.161, z: -0.137}, orientation: Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}}, //palm + Posef { position: Vector3f {x: -0.548, y: 0.161, z: -0.137}, orientation: Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}}, + Posef { position: Vector3f {x: -0.529, y: -0.198, z: -0.126}, orientation: Quaternionf {x: -0.744, y: -0.530, z: 0.156,w: -0.376}}, + Posef { position: Vector3f {x: -0.533, y: -0.175, z: -0.090}, orientation: Quaternionf {x: -0.786, y: -0.550, z: 0.126,w: -0.254}}, + Posef { position: Vector3f {x: -0.544, y: -0.158, z: -0.069}, orientation: Quaternionf {x: -0.729, y: -0.564, z: 0.027,w: -0.387}}, + Posef { position: Vector3f {x: -0.557, y: -0.150, z: -0.065}, orientation: Quaternionf {x: -0.585, y: -0.548, z: -0.140,w: -0.582}}, + Posef { position: Vector3f {x: -0.521, y: -0.182, z: -0.136}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317,w: -0.376}}, + Posef { position: Vector3f {x: -0.550, y: -0.135, z: -0.102}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317,w: -0.376}}, + Posef { position: Vector3f {x: -0.571, y: -0.112, z: -0.082}, orientation: Quaternionf {x: -0.244, y: -0.843, z: 0.256,w: -0.404}}, + Posef { position: Vector3f {x: -0.585, y: -0.102, z: -0.070}, orientation: Quaternionf {x: -0.200, y: -0.866, z: 0.165,w: -0.428}}, + Posef { position: Vector3f {x: -0.593, y: -0.098, z: -0.064}, orientation: Quaternionf {x: -0.172, y: -0.874, z: 0.110,w: -0.440}}, + Posef { position: Vector3f {x: -0.527, y: -0.178, z: -0.144}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370,w: -0.401}}, + Posef { position: Vector3f {x: -0.559, y: -0.132, z: -0.119}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370,w: -0.401}}, + Posef { position: Vector3f {x: -0.582, y: -0.101, z: -0.104}, orientation: Quaternionf {x: -0.175, y: -0.809, z: 0.371,w: -0.420}}, + Posef { position: Vector3f {x: -0.599, y: -0.089, z: -0.092}, orientation: Quaternionf {x: -0.109, y: -0.856, z: 0.245,w: -0.443}}, + Posef { position: Vector3f {x: -0.608, y: -0.084, z: -0.086}, orientation: Quaternionf {x: -0.075, y: -0.871, z: 0.180,w: -0.450}}, + Posef { position: Vector3f {x: -0.535, y: -0.178, z: -0.152}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408,w: -0.445}}, + Posef { position: Vector3f {x: -0.568, y: -0.136, z: -0.137}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408,w: -0.445}}, + Posef { position: Vector3f {x: -0.590, y: -0.106, z: -0.130}, orientation: Quaternionf {x: -0.131, y: -0.762, z: 0.432,w: -0.464}}, + Posef { position: Vector3f {x: -0.607, y: -0.092, z: -0.122}, orientation: Quaternionf {x: -0.071, y: -0.810, z: 0.332,w: -0.477}}, + Posef { position: Vector3f {x: -0.617, y: -0.086, z: -0.117}, orientation: Quaternionf {x: -0.029, y: -0.836, z: 0.260,w: -0.482}}, + Posef { position: Vector3f {x: -0.544, y: -0.183, z: -0.159}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481,w: -0.452}}, + Posef { position: Vector3f {x: -0.576, y: -0.143, z: -0.152}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481,w: -0.452}}, + Posef { position: Vector3f {x: -0.594, y: -0.119, z: -0.154}, orientation: Quaternionf {x: -0.061, y: -0.684, z: 0.534,w: -0.493}}, + Posef { position: Vector3f {x: -0.607, y: -0.108, z: -0.152}, orientation: Quaternionf {x: 0.002, y: -0.745, z: 0.444,w: -0.498}}, + Posef { position: Vector3f {x: -0.616, y: -0.102, z: -0.150}, orientation: Quaternionf {x: 0.045, y: -0.780, z: 0.378,w: -0.496}}, + ]; + //cursed wrist math + let wrist_dist = Vec3{ x: 0.01, y: -0.05, z: 0.0 }; + let huh = Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}.to_quat(); + let why: Vec3 = huh.mul_vec3(wrist_dist); + + let offset = Vec3 { x: 0.548, y: 1.0, z: 0.137 }; + let palm = hand_pose[HandJoint::PALM]; + gizmos.sphere(palm.position.to_vec3() + offset, palm.orientation.to_quat(), 0.01, Color::WHITE); + let wrist = hand_pose[HandJoint::WRIST]; + gizmos.sphere(palm.position.to_vec3() + offset + why, palm.orientation.to_quat(), 0.01, Color::GRAY); + let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; + gizmos.sphere(thumb_meta.position.to_vec3() + offset, thumb_meta.orientation.to_quat(), 0.01, Color::RED); + let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; + gizmos.sphere(thumb_prox.position.to_vec3() + offset, thumb_prox.orientation.to_quat(), 0.008, Color::RED); + let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; + gizmos.sphere(thumb_dist.position.to_vec3() + offset, thumb_dist.orientation.to_quat(), 0.006, Color::RED); + let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; + gizmos.sphere(thumb_tip.position.to_vec3() + offset, thumb_tip.orientation.to_quat(), 0.004, Color::RED); + + let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; + gizmos.sphere(index_meta.position.to_vec3() + offset, index_meta.orientation.to_quat(), 0.01, Color::ORANGE); + let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; + gizmos.sphere(index_prox.position.to_vec3() + offset, index_prox.orientation.to_quat(), 0.008, Color::ORANGE); + let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; + gizmos.sphere(index_inter.position.to_vec3() + offset, index_inter.orientation.to_quat(), 0.006, Color::ORANGE); + let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; + gizmos.sphere(index_dist.position.to_vec3() + offset, index_dist.orientation.to_quat(), 0.004, Color::ORANGE); + let index_tip = hand_pose[HandJoint::INDEX_TIP]; + gizmos.sphere(index_tip.position.to_vec3() + offset, index_tip.orientation.to_quat(), 0.002, Color::ORANGE); + + let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; + gizmos.sphere(middle_meta.position.to_vec3() + offset, middle_meta.orientation.to_quat(), 0.01, Color::YELLOW); + let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; + gizmos.sphere(middle_prox.position.to_vec3() + offset, middle_prox.orientation.to_quat(), 0.008, Color::YELLOW); + let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; + gizmos.sphere(middle_inter.position.to_vec3() + offset, middle_inter.orientation.to_quat(), 0.006, Color::YELLOW); + let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; + gizmos.sphere(middle_dist.position.to_vec3() + offset, middle_dist.orientation.to_quat(), 0.004, Color::YELLOW); + let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; + gizmos.sphere(middle_tip.position.to_vec3() + offset, middle_tip.orientation.to_quat(), 0.002, Color::YELLOW); + + let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; + gizmos.sphere(ring_meta.position.to_vec3() + offset, ring_meta.orientation.to_quat(), 0.01, Color::GREEN); + let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; + gizmos.sphere(ring_prox.position.to_vec3() + offset, ring_prox.orientation.to_quat(), 0.008, Color::GREEN); + let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; + gizmos.sphere(ring_inter.position.to_vec3() + offset, ring_inter.orientation.to_quat(), 0.006, Color::GREEN); + let ring_dist = hand_pose[HandJoint::RING_DISTAL]; + gizmos.sphere(ring_dist.position.to_vec3() + offset, ring_dist.orientation.to_quat(), 0.004, Color::GREEN); + let ring_tip = hand_pose[HandJoint::RING_TIP]; + gizmos.sphere(ring_tip.position.to_vec3() + offset, ring_tip.orientation.to_quat(), 0.002, Color::GREEN); + + let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; + gizmos.sphere(little_meta.position.to_vec3() + offset, little_meta.orientation.to_quat(), 0.01, Color::BLUE); + let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; + gizmos.sphere(little_prox.position.to_vec3() + offset, little_prox.orientation.to_quat(), 0.008, Color::BLUE); + let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; + gizmos.sphere(little_inter.position.to_vec3() + offset, little_inter.orientation.to_quat(), 0.006, Color::BLUE); + let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; + gizmos.sphere(little_dist.position.to_vec3() + offset, little_dist.orientation.to_quat(), 0.004, Color::BLUE); + let little_tip = hand_pose[HandJoint::LITTLE_TIP]; + gizmos.sphere(little_tip.position.to_vec3() + offset, little_tip.orientation.to_quat(), 0.002, Color::BLUE); + + + + + + +} + + fn spawn_controllers_example(mut commands: Commands) { //left hand commands.spawn(( diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs new file mode 100644 index 0000000..0fcd267 --- /dev/null +++ b/src/xr_input/hand.rs @@ -0,0 +1,3 @@ +pub struct Hand { + pub HandJointPoses: [Posef; 26] +} \ No newline at end of file From e1f19487263004f528c20301fe6235bc19f71a03 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Wed, 27 Sep 2023 15:40:29 -0400 Subject: [PATCH 06/66] basics of ray interactors --- examples/xr.rs | 11 ++- src/xr_input/interactions.rs | 157 ++++++++++++++++++++++++++++++----- src/xr_input/trackers.rs | 36 ++++++-- 3 files changed, 170 insertions(+), 34 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 36e835c..e8252f4 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -5,13 +5,13 @@ use bevy_openxr::input::XrInput; use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; use bevy_openxr::xr_input::interactions::{ - draw_interaction_gizmos, hover_interaction, XRDirectInteractor, XRInteractable, - XRInteractableState, XRInteractorState, + draw_interaction_gizmos, direct_interaction, XRDirectInteractor, XRInteractable, + XRInteractableState, XRInteractorState, XRRayInteractor, ray_interaction, }; 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, + OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, AimPose, }; use bevy_openxr::xr_input::Hand; use bevy_openxr::DefaultXrPlugins; @@ -30,7 +30,8 @@ fn main() { .add_systems(Startup, spawn_controllers_example) .insert_resource(PrototypeLocomotionConfig::default()) .add_systems(Update, draw_interaction_gizmos) - .add_systems(Update, hover_interaction) + .add_systems(Update, direct_interaction) + .add_systems(Update, ray_interaction) .add_systems(Update, prototype_interaction_input) .run(); } @@ -95,6 +96,8 @@ fn spawn_controllers_example(mut commands: Commands) { OpenXRTracker, SpatialBundle::default(), XRDirectInteractor, + XRRayInteractor, + AimPose(Transform::default()), XRInteractorState::default(), )); //right hand diff --git a/src/xr_input/interactions.rs b/src/xr_input/interactions.rs index 2fa5c35..a36d832 100644 --- a/src/xr_input/interactions.rs +++ b/src/xr_input/interactions.rs @@ -1,11 +1,17 @@ use std::f32::consts::PI; use bevy::prelude::{ - info, Color, Component, Gizmos, GlobalTransform, Quat, Query, Vec3, With, Without, + info, Color, Component, 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 enum XRInteractableState { Idle, @@ -40,10 +46,18 @@ pub fn draw_interaction_gizmos( (With, Without), >, interactor_query: Query< - (&GlobalTransform, &XRInteractorState), - (With, Without), + ( + &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 { @@ -54,25 +68,49 @@ pub fn draw_interaction_gizmos( gizmos.sphere(transform.translation, transform.rotation, 0.1, color); } - for (interactor_global_transform, interactor_state) in interactor_query.iter() { - let mut transform = interactor_global_transform.compute_transform(); - transform.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), - ); - transform.rotation = quat; - let color = match interactor_state { - XRInteractorState::Idle => Color::BLUE, - XRInteractorState::Selecting => Color::PURPLE, - }; - gizmos.cuboid(transform, 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 => (), + } } } -pub fn hover_interaction( +pub fn direct_interaction( mut interactable_query: Query< (&GlobalTransform, &mut XRInteractableState), (With, Without), @@ -82,9 +120,7 @@ pub fn hover_interaction( (With, Without), >, ) { - for (xr_interactable_global_transform, mut state) in - interactable_query.iter_mut() - { + for (xr_interactable_global_transform, mut state) in interactable_query.iter_mut() { let mut hovered = false; let mut selected = false; for (interactor_global_transform, interactor_state) in interactor_query.iter() { @@ -100,7 +136,6 @@ pub fn hover_interaction( ) < (size * size) * 2.0 { - //check for selections first match interactor_state { XRInteractorState::Idle => hovered = true, @@ -121,3 +156,79 @@ pub fn hover_interaction( } } } + +pub fn ray_interaction( + mut interactable_query: Query< + (&GlobalTransform, &mut XRInteractableState), + (With, Without), + >, + interactor_query: Query< + (&GlobalTransform, &XRInteractorState, Option<&AimPose>), + (With, Without), + >, + tracking_root_query: Query<(&mut Transform, With)>, +) { + for (xr_interactable_global_transform, mut state) in interactable_query.iter_mut() { + let mut hovered = false; + let mut selected = false; + for (interactor_global_transform, interactor_state, aim) in interactor_query.iter() { + //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 => hovered = true, + XRInteractorState::Selecting => { + selected = true; + } + } + } + } + None => info!("no aim pose"), + } + } + //check what we found + //also i dont like this + if selected { + *state = XRInteractableState::Select; + } else if hovered { + *state = XRInteractableState::Hover; + } else { + *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/trackers.rs b/src/xr_input/trackers.rs index f33e6ee..cbd4697 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<(Entity), Added>, @@ -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 aim_pose = left_controller_query.get_single_mut().unwrap().1; + match 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,7 +92,8 @@ 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(); @@ -84,4 +107,3 @@ pub fn update_open_xr_controllers( right_controller_query.get_single_mut().unwrap().0.rotation = right.0.pose.orientation.to_quat(); } - From 6e83b887be28d6f38962d5f13293b580b27ee28b Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Wed, 27 Sep 2023 23:43:03 -0400 Subject: [PATCH 07/66] interactions, and grabbables --- examples/xr.rs | 59 +++++++++-- src/xr_input/interactions.rs | 188 ++++++++++++++++++++--------------- 2 files changed, 159 insertions(+), 88 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index e8252f4..68b7b5d 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -5,13 +5,13 @@ use bevy_openxr::input::XrInput; use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; use bevy_openxr::xr_input::interactions::{ - draw_interaction_gizmos, direct_interaction, XRDirectInteractor, XRInteractable, - XRInteractableState, XRInteractorState, XRRayInteractor, ray_interaction, + interactions, draw_interaction_gizmos, update_interactable_states, InteractionEvent, + XRDirectInteractor, XRInteractable, XRInteractableState, XRInteractorState, XRRayInteractor, }; 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, + AimPose, OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, }; use bevy_openxr::xr_input::Hand; use bevy_openxr::DefaultXrPlugins; @@ -29,10 +29,15 @@ fn main() { .add_systems(Update, proto_locomotion) .add_systems(Startup, spawn_controllers_example) .insert_resource(PrototypeLocomotionConfig::default()) - .add_systems(Update, draw_interaction_gizmos) - .add_systems(Update, direct_interaction) - .add_systems(Update, ray_interaction) + .add_systems( + Update, + draw_interaction_gizmos.after(update_interactable_states), + ) + .add_systems(Update, interactions) .add_systems(Update, prototype_interaction_input) + .add_systems(Update, update_interactable_states.after(interactions)) + .add_systems(Update, update_grabbables.after(update_interactable_states)) + .add_event::() .run(); } @@ -85,6 +90,7 @@ fn setup( }, XRInteractable, XRInteractableState::default(), + Grabbable, )); } @@ -95,7 +101,6 @@ fn spawn_controllers_example(mut commands: Commands) { OpenXRController, OpenXRTracker, SpatialBundle::default(), - XRDirectInteractor, XRRayInteractor, AimPose(Transform::default()), XRInteractorState::default(), @@ -128,7 +133,7 @@ fn prototype_interaction_input( mut left_interactor_query: Query< (&mut XRInteractorState), ( - With, + With, With, Without, ), @@ -155,3 +160,41 @@ fn prototype_interaction_input( *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, With, 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) => { + info!("its a direct interactor?"); + info!( + "before gT: {:?}, iT: {:?}", + grabbable_transform, interactor_transform + ); + *grabbable_transform.0 = interactor_transform.0.compute_transform(); + info!( + "after gT: {:?}, iT: {:?}", + grabbable_transform, interactor_transform + ); + } + Err(_) => info!("not a direct interactor"), + } + } + Err(_) => { + info!("not a grabbable?") + } + } + } +} diff --git a/src/xr_input/interactions.rs b/src/xr_input/interactions.rs index a36d832..86a35e5 100644 --- a/src/xr_input/interactions.rs +++ b/src/xr_input/interactions.rs @@ -1,7 +1,8 @@ use std::f32::consts::PI; use bevy::prelude::{ - info, Color, Component, Gizmos, GlobalTransform, Quat, Query, Transform, Vec3, With, Without, + info, Color, Component, Entity, Event, EventReader, EventWriter, Gizmos, GlobalTransform, Quat, + Query, Transform, Vec3, With, Without, }; use super::trackers::{AimPose, OpenXRTrackingRoot}; @@ -12,7 +13,7 @@ pub struct XRDirectInteractor; #[derive(Component)] pub struct XRRayInteractor; -#[derive(Component)] +#[derive(Component, Clone, Copy)] pub enum XRInteractableState { Idle, Hover, @@ -110,46 +111,113 @@ pub fn draw_interaction_gizmos( } } -pub fn direct_interaction( +#[derive(Event)] +pub struct InteractionEvent { + pub interactor: Entity, + pub interactable: Entity, + pub interactable_state: XRInteractableState, +} + +pub fn interactions( mut interactable_query: Query< - (&GlobalTransform, &mut XRInteractableState), + (&GlobalTransform, &mut XRInteractableState, Entity), (With, Without), >, interactor_query: Query< - (&GlobalTransform, &XRInteractorState), - (With, Without), + ( + &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, mut state) in interactable_query.iter_mut() { + for (xr_interactable_global_transform, mut state, interactable_entity) in + interactable_query.iter_mut() + { let mut hovered = false; - let mut selected = false; - for (interactor_global_transform, interactor_state) in interactor_query.iter() { - //check for sphere overlaps - let size = 0.1; - if interactor_global_transform - .compute_transform() - .translation - .distance_squared( - xr_interactable_global_transform + 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, - ) - < (size * size) * 2.0 - { - //check for selections first - match interactor_state { - XRInteractorState::Idle => hovered = true, - XRInteractorState::Selecting => { - selected = true; + .translation + .distance_squared( + xr_interactable_global_transform + .compute_transform() + .translation, + ) + < (size * size) * 2.0 + { + //check for selections first + match interactor_state { + XRInteractorState::Idle => hovered = true, + XRInteractorState::Selecting => { + //welp now I gota actually make things do stuff lol + 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 => hovered = true, + XRInteractorState::Selecting => { + //welp now I gota actually make things do stuff lol + let event = InteractionEvent { + interactor: interactor_entity, + interactable: interactable_entity, + interactable_state: XRInteractableState::Select, + }; + writer.send(event); + } + } + } + } + None => info!("no aim pose"), + } + } + None => (), } } - //check what we found - //also i dont like this - if selected { - *state = XRInteractableState::Select; - } else if hovered { + //still hate this + if hovered { *state = XRInteractableState::Hover; } else { *state = XRInteractableState::Idle; @@ -157,58 +225,18 @@ pub fn direct_interaction( } } -pub fn ray_interaction( - mut interactable_query: Query< - (&GlobalTransform, &mut XRInteractableState), - (With, Without), - >, - interactor_query: Query< - (&GlobalTransform, &XRInteractorState, Option<&AimPose>), - (With, Without), - >, - tracking_root_query: Query<(&mut Transform, With)>, +pub fn update_interactable_states( + mut events: EventReader, + mut interactable_query: Query<(Entity, &mut XRInteractableState), (With)>, ) { - for (xr_interactable_global_transform, mut state) in interactable_query.iter_mut() { - let mut hovered = false; - let mut selected = false; - for (interactor_global_transform, interactor_state, aim) in interactor_query.iter() { - //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 => hovered = true, - XRInteractorState::Selecting => { - selected = true; - } - } - } - } - None => info!("no aim pose"), + for event in events.read() { + //lets change the state? + match interactable_query.get_mut(event.interactable) { + Ok((_entity, mut entity_state)) => { + *entity_state = event.interactable_state; + } + Err(_) => { } - } - //check what we found - //also i dont like this - if selected { - *state = XRInteractableState::Select; - } else if hovered { - *state = XRInteractableState::Hover; - } else { - *state = XRInteractableState::Idle; } } } From 88838b595b9b06ad5b02d460ecf72ccee97e47b5 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sat, 30 Sep 2023 23:02:30 -0400 Subject: [PATCH 08/66] jank socket interactor --- examples/xr.rs | 30 +++++++++----- src/xr_input/interactions.rs | 80 +++++++++++++++++++++++++++++++++++- 2 files changed, 98 insertions(+), 12 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 68b7b5d..35b0800 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -5,8 +5,9 @@ use bevy_openxr::input::XrInput; use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; use bevy_openxr::xr_input::interactions::{ - interactions, draw_interaction_gizmos, update_interactable_states, InteractionEvent, - XRDirectInteractor, XRInteractable, XRInteractableState, XRInteractorState, XRRayInteractor, + draw_interaction_gizmos, draw_socket_gizmos, interactions, update_interactable_states, + InteractionEvent, XRDirectInteractor, XRInteractable, XRInteractableState, XRInteractorState, + XRRayInteractor, XRSocketInteractor, socket_interactions, }; use bevy_openxr::xr_input::oculus_touch::OculusController; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; @@ -33,7 +34,9 @@ fn main() { Update, draw_interaction_gizmos.after(update_interactable_states), ) + .add_systems(Update, draw_socket_gizmos.after(update_interactable_states)) .add_systems(Update, interactions) + .add_systems(Update, socket_interactions) .add_systems(Update, prototype_interaction_input) .add_systems(Update, update_interactable_states.after(interactions)) .add_systems(Update, update_grabbables.after(update_interactable_states)) @@ -60,13 +63,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 { @@ -167,7 +173,11 @@ pub struct Grabbable; pub fn update_grabbables( mut events: EventReader, mut grabbable_query: Query<(&mut Transform, With, Without)>, - interactor_query: Query<(&GlobalTransform, With, Without)>, + interactor_query: Query<( + &GlobalTransform, + With, + Without, + )>, ) { //so basically the idea is to try all the events? for event in events.read() { diff --git a/src/xr_input/interactions.rs b/src/xr_input/interactions.rs index 86a35e5..db3dfc2 100644 --- a/src/xr_input/interactions.rs +++ b/src/xr_input/interactions.rs @@ -13,6 +13,9 @@ pub struct XRDirectInteractor; #[derive(Component)] pub struct XRRayInteractor; +#[derive(Component)] +pub struct XRSocketInteractor; + #[derive(Component, Clone, Copy)] pub enum XRInteractableState { Idle, @@ -40,6 +43,27 @@ impl Default for XRInteractorState { #[derive(Component)] pub struct XRInteractable; +//i guess really these should be seperate +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< @@ -118,6 +142,59 @@ pub struct InteractionEvent { pub interactable_state: XRInteractableState, } +//yeah these need to be seperate somehow +pub fn socket_interactions( + mut 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 interactbles + 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 => (), //welp this wont work, + XRInteractorState::Selecting => { + //welp now I gota actually make things do stuff lol + let event = InteractionEvent { + interactor: socket.2, + interactable: interactable.2, + interactable_state: XRInteractableState::Select, + }; + writer.send(event); + } + } + } + } + } +} + pub fn interactions( mut interactable_query: Query< (&GlobalTransform, &mut XRInteractableState, Entity), @@ -235,8 +312,7 @@ pub fn update_interactable_states( Ok((_entity, mut entity_state)) => { *entity_state = event.interactable_state; } - Err(_) => { - } + Err(_) => {} } } } From d80506c43c0a1957ee3cd3dbc410149d52db86b9 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sun, 1 Oct 2023 00:22:42 -0400 Subject: [PATCH 09/66] this is a little better --- examples/xr.rs | 56 ++++++++++++++++------------- src/xr_input/interactions.rs | 69 +++++++++++++++++++++++++----------- 2 files changed, 79 insertions(+), 46 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 35b0800..fd8f2ea 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -5,9 +5,9 @@ use bevy_openxr::input::XrInput; use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; use bevy_openxr::xr_input::interactions::{ - draw_interaction_gizmos, draw_socket_gizmos, interactions, update_interactable_states, - InteractionEvent, XRDirectInteractor, XRInteractable, XRInteractableState, XRInteractorState, - XRRayInteractor, XRSocketInteractor, socket_interactions, + draw_interaction_gizmos, draw_socket_gizmos, interactions, socket_interactions, + update_interactable_states, InteractionEvent, XRDirectInteractor, XRInteractable, + XRInteractableState, XRInteractorState, XRRayInteractor, XRSocketInteractor, }; use bevy_openxr::xr_input::oculus_touch::OculusController; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; @@ -35,10 +35,13 @@ fn main() { draw_interaction_gizmos.after(update_interactable_states), ) .add_systems(Update, draw_socket_gizmos.after(update_interactable_states)) - .add_systems(Update, interactions) - .add_systems(Update, socket_interactions) + .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.after(interactions)) + .add_systems(Update, update_interactable_states) .add_systems(Update, update_grabbables.after(update_interactable_states)) .add_event::() .run(); @@ -173,37 +176,40 @@ pub struct Grabbable; pub fn update_grabbables( mut events: EventReader, mut grabbable_query: Query<(&mut Transform, With, Without)>, - interactor_query: Query<( - &GlobalTransform, - 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"); + // info!("some event"); match grabbable_query.get_mut(event.interactable) { Ok(mut grabbable_transform) => { - info!("we got a grabbable"); + // info!("we got a grabbable"); //now we need the location of our interactor match interactor_query.get(event.interactor) { Ok(interactor_transform) => { - info!("its a direct interactor?"); - info!( - "before gT: {:?}, iT: {:?}", - grabbable_transform, interactor_transform - ); - *grabbable_transform.0 = interactor_transform.0.compute_transform(); - info!( - "after gT: {:?}, iT: {:?}", - grabbable_transform, interactor_transform - ); + match interactor_transform.1 { + XRInteractorState::Idle => (), + XRInteractorState::Selecting => { + // info!("its a direct interactor?"); + // info!( + // "before gT: {:?}, iT: {:?}", + // grabbable_transform, interactor_transform + // ); + *grabbable_transform.0 = interactor_transform.0.compute_transform(); + // info!( + // "after gT: {:?}, iT: {:?}", + // grabbable_transform, interactor_transform + // ); + } + } + } + Err(_) => { + // info!("not a direct interactor") } - Err(_) => info!("not a direct interactor"), } } Err(_) => { - info!("not a grabbable?") + // info!("not a grabbable?") } } } diff --git a/src/xr_input/interactions.rs b/src/xr_input/interactions.rs index db3dfc2..425d8d6 100644 --- a/src/xr_input/interactions.rs +++ b/src/xr_input/interactions.rs @@ -16,7 +16,7 @@ pub struct XRRayInteractor; #[derive(Component)] pub struct XRSocketInteractor; -#[derive(Component, Clone, Copy)] +#[derive(Component, Clone, Copy, PartialEq, PartialOrd, Debug)] pub enum XRInteractableState { Idle, Hover, @@ -78,7 +78,7 @@ pub fn draw_interaction_gizmos( Option<&XRRayInteractor>, Option<&AimPose>, ), - (Without), + Without, >, tracking_root_query: Query<(&mut Transform, With)>, ) { @@ -144,7 +144,7 @@ pub struct InteractionEvent { //yeah these need to be seperate somehow pub fn socket_interactions( - mut interactable_query: Query< + interactable_query: Query< (&GlobalTransform, &mut XRInteractableState, Entity), (With, Without), >, @@ -155,7 +155,7 @@ pub fn socket_interactions( Entity, &XRSocketInteractor, ), - (Without), + Without, >, mut writer: EventWriter, ) { @@ -179,7 +179,15 @@ pub fn socket_interactions( { //check for selections first match interactor_state { - XRInteractorState::Idle => (), //welp this wont work, + XRInteractorState::Idle => { + //welp now I gota actually make things do stuff lol + let event = InteractionEvent { + interactor: socket.2, + interactable: interactable.2, + interactable_state: XRInteractableState::Hover, + }; + writer.send(event); + } XRInteractorState::Selecting => { //welp now I gota actually make things do stuff lol let event = InteractionEvent { @@ -196,8 +204,8 @@ pub fn socket_interactions( } pub fn interactions( - mut interactable_query: Query< - (&GlobalTransform, &mut XRInteractableState, Entity), + interactable_query: Query< + (&GlobalTransform, Entity), (With, Without), >, interactor_query: Query< @@ -209,15 +217,12 @@ pub fn interactions( Option<&XRRayInteractor>, Option<&AimPose>, ), - (Without), + Without, >, tracking_root_query: Query<(&mut Transform, With)>, mut writer: EventWriter, ) { - for (xr_interactable_global_transform, mut state, interactable_entity) in - interactable_query.iter_mut() - { - let mut hovered = false; + 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() { @@ -237,7 +242,15 @@ pub fn interactions( { //check for selections first match interactor_state { - XRInteractorState::Idle => hovered = true, + XRInteractorState::Idle => { + //welp now I gota actually make things do stuff lol + let event = InteractionEvent { + interactor: interactor_entity, + interactable: interactable_entity, + interactable_state: XRInteractableState::Hover, + }; + writer.send(event); + } XRInteractorState::Selecting => { //welp now I gota actually make things do stuff lol let event = InteractionEvent { @@ -274,7 +287,15 @@ pub fn interactions( ) { //check for selections first match interactor_state { - XRInteractorState::Idle => hovered = true, + XRInteractorState::Idle => { + //welp now I gota actually make things do stuff lol + let event = InteractionEvent { + interactor: interactor_entity, + interactable: interactable_entity, + interactable_state: XRInteractableState::Hover, + }; + writer.send(event); + } XRInteractorState::Selecting => { //welp now I gota actually make things do stuff lol let event = InteractionEvent { @@ -293,23 +314,29 @@ pub fn interactions( None => (), } } - //still hate this - if hovered { - *state = XRInteractableState::Hover; - } else { - *state = XRInteractableState::Idle; - } } } pub fn update_interactable_states( mut events: EventReader, - mut interactable_query: Query<(Entity, &mut XRInteractableState), (With)>, + mut interactable_query: Query<(Entity, &mut XRInteractableState), With>, ) { + //i legit hate this haha but it works + for (_entity, mut state) in interactable_query.iter_mut() { + *state = XRInteractableState::Idle; + } + for event in events.read() { //lets change the state? match interactable_query.get_mut(event.interactable) { Ok((_entity, mut entity_state)) => { + 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(_) => {} From b487bf6cbb3d79593091ffed003532a2bb4e6ff0 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sun, 1 Oct 2023 23:42:43 -0400 Subject: [PATCH 10/66] this works well enough for now --- examples/xr.rs | 3 ++- src/xr_input/interactions.rs | 37 ++++++++++++++++++++---------------- 2 files changed, 23 insertions(+), 17 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index fd8f2ea..2aed227 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -6,7 +6,7 @@ use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; use bevy_openxr::xr_input::interactions::{ draw_interaction_gizmos, draw_socket_gizmos, interactions, socket_interactions, - update_interactable_states, InteractionEvent, XRDirectInteractor, XRInteractable, + update_interactable_states, InteractionEvent, Touched, XRDirectInteractor, XRInteractable, XRInteractableState, XRInteractorState, XRRayInteractor, XRSocketInteractor, }; use bevy_openxr::xr_input::oculus_touch::OculusController; @@ -100,6 +100,7 @@ fn setup( XRInteractable, XRInteractableState::default(), Grabbable, + Touched(false), )); } diff --git a/src/xr_input/interactions.rs b/src/xr_input/interactions.rs index 425d8d6..9660839 100644 --- a/src/xr_input/interactions.rs +++ b/src/xr_input/interactions.rs @@ -16,6 +16,9 @@ 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, @@ -43,7 +46,6 @@ impl Default for XRInteractorState { #[derive(Component)] pub struct XRInteractable; -//i guess really these should be seperate pub fn draw_socket_gizmos( mut gizmos: Gizmos, interactor_query: Query<( @@ -142,7 +144,6 @@ pub struct InteractionEvent { pub interactable_state: XRInteractableState, } -//yeah these need to be seperate somehow pub fn socket_interactions( interactable_query: Query< (&GlobalTransform, &mut XRInteractableState, Entity), @@ -160,7 +161,7 @@ pub fn socket_interactions( mut writer: EventWriter, ) { for interactable in interactable_query.iter() { - //for the interactbles + //for the interactables for socket in interactor_query.iter() { let interactor_global_transform = socket.0; let xr_interactable_global_transform = interactable.0; @@ -180,7 +181,6 @@ pub fn socket_interactions( //check for selections first match interactor_state { XRInteractorState::Idle => { - //welp now I gota actually make things do stuff lol let event = InteractionEvent { interactor: socket.2, interactable: interactable.2, @@ -189,7 +189,6 @@ pub fn socket_interactions( writer.send(event); } XRInteractorState::Selecting => { - //welp now I gota actually make things do stuff lol let event = InteractionEvent { interactor: socket.2, interactable: interactable.2, @@ -243,7 +242,6 @@ pub fn interactions( //check for selections first match interactor_state { XRInteractorState::Idle => { - //welp now I gota actually make things do stuff lol let event = InteractionEvent { interactor: interactor_entity, interactable: interactable_entity, @@ -252,7 +250,6 @@ pub fn interactions( writer.send(event); } XRInteractorState::Selecting => { - //welp now I gota actually make things do stuff lol let event = InteractionEvent { interactor: interactor_entity, interactable: interactable_entity, @@ -288,7 +285,6 @@ pub fn interactions( //check for selections first match interactor_state { XRInteractorState::Idle => { - //welp now I gota actually make things do stuff lol let event = InteractionEvent { interactor: interactor_entity, interactable: interactable_entity, @@ -297,7 +293,6 @@ pub fn interactions( writer.send(event); } XRInteractorState::Selecting => { - //welp now I gota actually make things do stuff lol let event = InteractionEvent { interactor: interactor_entity, interactable: interactable_entity, @@ -319,17 +314,21 @@ pub fn interactions( pub fn update_interactable_states( mut events: EventReader, - mut interactable_query: Query<(Entity, &mut XRInteractableState), With>, + mut interactable_query: Query< + (Entity, &mut XRInteractableState, &mut Touched), + With, + >, ) { - //i legit hate this haha but it works - for (_entity, mut state) in interactable_query.iter_mut() { - *state = XRInteractableState::Idle; + //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? + //lets change the state match interactable_query.get_mut(event.interactable) { - Ok((_entity, mut entity_state)) => { + 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: {:?}", @@ -342,6 +341,12 @@ pub fn update_interactable_states( 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 { From 6fc41ecbfca2bdb0bece4b50f9287d8a3ca71be3 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sun, 1 Oct 2023 23:44:00 -0400 Subject: [PATCH 11/66] cleaning --- examples/xr.rs | 8 -------- 1 file changed, 8 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 2aed227..0b15b9d 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -192,15 +192,7 @@ pub fn update_grabbables( XRInteractorState::Idle => (), XRInteractorState::Selecting => { // info!("its a direct interactor?"); - // info!( - // "before gT: {:?}, iT: {:?}", - // grabbable_transform, interactor_transform - // ); *grabbable_transform.0 = interactor_transform.0.compute_transform(); - // info!( - // "after gT: {:?}, iT: {:?}", - // grabbable_transform, interactor_transform - // ); } } } From 903dc2a5da68f86d31e45b458f56515fa552916f Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sun, 1 Oct 2023 23:48:58 -0400 Subject: [PATCH 12/66] added right hand aim poses --- src/xr_input/trackers.rs | 23 ++++++++++++++++++----- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/src/xr_input/trackers.rs b/src/xr_input/trackers.rs index cbd4697..233acc6 100644 --- a/src/xr_input/trackers.rs +++ b/src/xr_input/trackers.rs @@ -74,8 +74,8 @@ pub fn update_open_xr_controllers( 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 aim_pose = left_controller_query.get_single_mut().unwrap().1; - match aim_pose { + 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(), @@ -95,8 +95,21 @@ pub fn update_open_xr_controllers( 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() @@ -105,5 +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(); } From ed9508756078c7cffd40a428c358652c281d7815 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 3 Oct 2023 14:28:33 -0400 Subject: [PATCH 13/66] well this is insane but it tracks the hand now --- examples/xr.rs | 158 ++++++++++++++++++++++++++++--------------------- 1 file changed, 92 insertions(+), 66 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 80eb3b7..1f3df09 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,3 +1,5 @@ +use std::f32::consts::PI; + use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; @@ -16,13 +18,13 @@ 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) - .add_systems(Update, spawn_skeleton) + .add_systems(Update, draw_skeleton_hand) .insert_resource(PrototypeLocomotionConfig::default()) .run(); } @@ -70,103 +72,127 @@ fn setup( },)); } -fn spawn_skeleton(mut commands: Commands, mut gizmos: Gizmos) { - let hand_pose: [Posef; 26] = [ - Posef { position: Vector3f {x: -0.548, y: -0.161, z: -0.137}, orientation: Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}}, //palm - Posef { position: Vector3f {x: -0.548, y: 0.161, z: -0.137}, orientation: Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}}, - Posef { position: Vector3f {x: -0.529, y: -0.198, z: -0.126}, orientation: Quaternionf {x: -0.744, y: -0.530, z: 0.156,w: -0.376}}, - Posef { position: Vector3f {x: -0.533, y: -0.175, z: -0.090}, orientation: Quaternionf {x: -0.786, y: -0.550, z: 0.126,w: -0.254}}, - Posef { position: Vector3f {x: -0.544, y: -0.158, z: -0.069}, orientation: Quaternionf {x: -0.729, y: -0.564, z: 0.027,w: -0.387}}, - Posef { position: Vector3f {x: -0.557, y: -0.150, z: -0.065}, orientation: Quaternionf {x: -0.585, y: -0.548, z: -0.140,w: -0.582}}, - Posef { position: Vector3f {x: -0.521, y: -0.182, z: -0.136}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317,w: -0.376}}, - Posef { position: Vector3f {x: -0.550, y: -0.135, z: -0.102}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317,w: -0.376}}, - Posef { position: Vector3f {x: -0.571, y: -0.112, z: -0.082}, orientation: Quaternionf {x: -0.244, y: -0.843, z: 0.256,w: -0.404}}, - Posef { position: Vector3f {x: -0.585, y: -0.102, z: -0.070}, orientation: Quaternionf {x: -0.200, y: -0.866, z: 0.165,w: -0.428}}, - Posef { position: Vector3f {x: -0.593, y: -0.098, z: -0.064}, orientation: Quaternionf {x: -0.172, y: -0.874, z: 0.110,w: -0.440}}, - Posef { position: Vector3f {x: -0.527, y: -0.178, z: -0.144}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370,w: -0.401}}, - Posef { position: Vector3f {x: -0.559, y: -0.132, z: -0.119}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370,w: -0.401}}, - Posef { position: Vector3f {x: -0.582, y: -0.101, z: -0.104}, orientation: Quaternionf {x: -0.175, y: -0.809, z: 0.371,w: -0.420}}, - Posef { position: Vector3f {x: -0.599, y: -0.089, z: -0.092}, orientation: Quaternionf {x: -0.109, y: -0.856, z: 0.245,w: -0.443}}, - Posef { position: Vector3f {x: -0.608, y: -0.084, z: -0.086}, orientation: Quaternionf {x: -0.075, y: -0.871, z: 0.180,w: -0.450}}, - Posef { position: Vector3f {x: -0.535, y: -0.178, z: -0.152}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408,w: -0.445}}, - Posef { position: Vector3f {x: -0.568, y: -0.136, z: -0.137}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408,w: -0.445}}, - Posef { position: Vector3f {x: -0.590, y: -0.106, z: -0.130}, orientation: Quaternionf {x: -0.131, y: -0.762, z: 0.432,w: -0.464}}, - Posef { position: Vector3f {x: -0.607, y: -0.092, z: -0.122}, orientation: Quaternionf {x: -0.071, y: -0.810, z: 0.332,w: -0.477}}, - Posef { position: Vector3f {x: -0.617, y: -0.086, z: -0.117}, orientation: Quaternionf {x: -0.029, y: -0.836, z: 0.260,w: -0.482}}, - Posef { position: Vector3f {x: -0.544, y: -0.183, z: -0.159}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481,w: -0.452}}, - Posef { position: Vector3f {x: -0.576, y: -0.143, z: -0.152}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481,w: -0.452}}, - Posef { position: Vector3f {x: -0.594, y: -0.119, z: -0.154}, orientation: Quaternionf {x: -0.061, y: -0.684, z: 0.534,w: -0.493}}, - Posef { position: Vector3f {x: -0.607, y: -0.108, z: -0.152}, orientation: Quaternionf {x: 0.002, y: -0.745, z: 0.444,w: -0.498}}, - Posef { position: Vector3f {x: -0.616, y: -0.102, z: -0.150}, orientation: Quaternionf {x: 0.045, y: -0.780, z: 0.378,w: -0.496}}, - ]; - //cursed wrist math - let wrist_dist = Vec3{ x: 0.01, y: -0.05, z: 0.0 }; - let huh = Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}.to_quat(); - let why: Vec3 = huh.mul_vec3(wrist_dist); +fn draw_skeleton_hand(mut commands: Commands, + mut gizmos: Gizmos, + right_controller_query: Query<( + &GlobalTransform, + With, +)>, ) { + //draw debug for controller grip center to match palm to + let right_transform = right_controller_query.get_single().unwrap().0; + let right_translation = right_transform.compute_transform().translation; + let right_quat = right_transform.compute_transform().rotation; + gizmos.sphere(right_translation, Quat::IDENTITY, 0.01, Color::PINK); + //we need to flip this i dont know why + let flip = Quat::from_rotation_x(PI); + let flap = Quat::from_rotation_z(-45.0*(PI/180.0)); + let controller_forward = right_quat.mul_quat(flip).mul_vec3(Vec3::Y); + let controller_backward = right_quat.mul_quat(flip).mul_quat(flap); + gizmos.ray(right_translation, controller_forward, Color::PINK); - let offset = Vec3 { x: 0.548, y: 1.0, z: 0.137 }; + let hand_pose: [Posef; 26] = [ + Posef { position: Vector3f {x: -0.548, y: -0.161, z: -0.137}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, //palm + Posef { position: Vector3f {x: -0.548, y: 0.161, z: -0.137}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, + Posef { position: Vector3f {x: -0.529, y: -0.198, z: -0.126}, orientation: Quaternionf {x: -0.744, y: -0.530, z: 0.156, w: -0.376}}, + Posef { position: Vector3f {x: -0.533, y: -0.175, z: -0.090}, orientation: Quaternionf {x: -0.786, y: -0.550, z: 0.126, w: -0.254}}, + Posef { position: Vector3f {x: -0.544, y: -0.158, z: -0.069}, orientation: Quaternionf {x: -0.729, y: -0.564, z: 0.027, w: -0.387}}, + Posef { position: Vector3f {x: -0.557, y: -0.150, z: -0.065}, orientation: Quaternionf {x: -0.585, y: -0.548, z: -0.140,w: -0.582}}, + Posef { position: Vector3f {x: -0.521, y: -0.182, z: -0.136}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317, w: -0.376}}, + Posef { position: Vector3f {x: -0.550, y: -0.135, z: -0.102}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317, w: -0.376}}, + Posef { position: Vector3f {x: -0.571, y: -0.112, z: -0.082}, orientation: Quaternionf {x: -0.244, y: -0.843, z: 0.256, w: -0.404}}, + Posef { position: Vector3f {x: -0.585, y: -0.102, z: -0.070}, orientation: Quaternionf {x: -0.200, y: -0.866, z: 0.165, w: -0.428}}, + Posef { position: Vector3f {x: -0.593, y: -0.098, z: -0.064}, orientation: Quaternionf {x: -0.172, y: -0.874, z: 0.110, w: -0.440}}, + Posef { position: Vector3f {x: -0.527, y: -0.178, z: -0.144}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370, w: -0.401}}, + Posef { position: Vector3f {x: -0.559, y: -0.132, z: -0.119}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370, w: -0.401}}, + Posef { position: Vector3f {x: -0.582, y: -0.101, z: -0.104}, orientation: Quaternionf {x: -0.175, y: -0.809, z: 0.371, w: -0.420}}, + Posef { position: Vector3f {x: -0.599, y: -0.089, z: -0.092}, orientation: Quaternionf {x: -0.109, y: -0.856, z: 0.245, w: -0.443}}, + Posef { position: Vector3f {x: -0.608, y: -0.084, z: -0.086}, orientation: Quaternionf {x: -0.075, y: -0.871, z: 0.180, w: -0.450}}, + Posef { position: Vector3f {x: -0.535, y: -0.178, z: -0.152}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408, w: -0.445}}, + Posef { position: Vector3f {x: -0.568, y: -0.136, z: -0.137}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408, w: -0.445}}, + Posef { position: Vector3f {x: -0.590, y: -0.106, z: -0.130}, orientation: Quaternionf {x: -0.131, y: -0.762, z: 0.432, w: -0.464}}, + Posef { position: Vector3f {x: -0.607, y: -0.092, z: -0.122}, orientation: Quaternionf {x: -0.071, y: -0.810, z: 0.332, w: -0.477}}, + Posef { position: Vector3f {x: -0.617, y: -0.086, z: -0.117}, orientation: Quaternionf {x: -0.029, y: -0.836, z: 0.260, w: -0.482}}, + Posef { position: Vector3f {x: -0.544, y: -0.183, z: -0.159}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481, w: -0.452}}, + Posef { position: Vector3f {x: -0.576, y: -0.143, z: -0.152}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481, w: -0.452}}, + Posef { position: Vector3f {x: -0.594, y: -0.119, z: -0.154}, orientation: Quaternionf {x: -0.061, y: -0.684, z: 0.534, w: -0.493}}, + Posef { position: Vector3f {x: -0.607, y: -0.108, z: -0.152}, orientation: Quaternionf {x: 0.002, y: -0.745, z: 0.444, w: -0.498}}, + Posef { position: Vector3f {x: -0.616, y: -0.102, z: -0.150}, orientation: Quaternionf {x: 0.045, y: -0.780, z: 0.378, w: -0.496}}, + ]; + + //cursed wrist math + let wrist_dist = Vec3{ x: -0.01, y: -0.040, z: -0.015}; + + //cursed offset + let palm_negation = Vec3 { x: 0.548, y: 0.161, z: 0.137 }; + let offset = right_translation; + + //old stuff dont touch for now let palm = hand_pose[HandJoint::PALM]; - gizmos.sphere(palm.position.to_vec3() + offset, palm.orientation.to_quat(), 0.01, Color::WHITE); - let wrist = hand_pose[HandJoint::WRIST]; - gizmos.sphere(palm.position.to_vec3() + offset + why, palm.orientation.to_quat(), 0.01, Color::GRAY); + gizmos.sphere(palm.position.to_vec3() + offset + palm_negation, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); + + let rotated_wfp = palm.position.to_vec3() + right_quat.mul_quat(flip).mul_vec3(wrist_dist); + gizmos.sphere(offset + palm_negation + rotated_wfp, palm.orientation.to_quat(), 0.01, Color::GRAY); + + let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; - gizmos.sphere(thumb_meta.position.to_vec3() + offset, thumb_meta.orientation.to_quat(), 0.01, Color::RED); + draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, palm_negation, offset); + let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; - gizmos.sphere(thumb_prox.position.to_vec3() + offset, thumb_prox.orientation.to_quat(), 0.008, Color::RED); + draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, palm_negation, offset); let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; - gizmos.sphere(thumb_dist.position.to_vec3() + offset, thumb_dist.orientation.to_quat(), 0.006, Color::RED); + draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, palm_negation, offset); let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; - gizmos.sphere(thumb_tip.position.to_vec3() + offset, thumb_tip.orientation.to_quat(), 0.004, Color::RED); + draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, palm_negation, offset); let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; - gizmos.sphere(index_meta.position.to_vec3() + offset, index_meta.orientation.to_quat(), 0.01, Color::ORANGE); + draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, palm_negation, offset); let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; - gizmos.sphere(index_prox.position.to_vec3() + offset, index_prox.orientation.to_quat(), 0.008, Color::ORANGE); + draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, palm_negation, offset); let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; - gizmos.sphere(index_inter.position.to_vec3() + offset, index_inter.orientation.to_quat(), 0.006, Color::ORANGE); + draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, palm_negation, offset); let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; - gizmos.sphere(index_dist.position.to_vec3() + offset, index_dist.orientation.to_quat(), 0.004, Color::ORANGE); + draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, palm_negation, offset); let index_tip = hand_pose[HandJoint::INDEX_TIP]; - gizmos.sphere(index_tip.position.to_vec3() + offset, index_tip.orientation.to_quat(), 0.002, Color::ORANGE); + draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, palm_negation, offset); let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; - gizmos.sphere(middle_meta.position.to_vec3() + offset, middle_meta.orientation.to_quat(), 0.01, Color::YELLOW); + draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, palm_negation, offset); let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; - gizmos.sphere(middle_prox.position.to_vec3() + offset, middle_prox.orientation.to_quat(), 0.008, Color::YELLOW); + draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, palm_negation, offset); let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; - gizmos.sphere(middle_inter.position.to_vec3() + offset, middle_inter.orientation.to_quat(), 0.006, Color::YELLOW); + draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, palm_negation, offset); let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; - gizmos.sphere(middle_dist.position.to_vec3() + offset, middle_dist.orientation.to_quat(), 0.004, Color::YELLOW); + draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, palm_negation, offset); let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; - gizmos.sphere(middle_tip.position.to_vec3() + offset, middle_tip.orientation.to_quat(), 0.002, Color::YELLOW); + draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, palm_negation, offset); let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; - gizmos.sphere(ring_meta.position.to_vec3() + offset, ring_meta.orientation.to_quat(), 0.01, Color::GREEN); + draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, palm_negation, offset); let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; - gizmos.sphere(ring_prox.position.to_vec3() + offset, ring_prox.orientation.to_quat(), 0.008, Color::GREEN); + draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, palm_negation, offset); let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; - gizmos.sphere(ring_inter.position.to_vec3() + offset, ring_inter.orientation.to_quat(), 0.006, Color::GREEN); + draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, palm_negation, offset); let ring_dist = hand_pose[HandJoint::RING_DISTAL]; - gizmos.sphere(ring_dist.position.to_vec3() + offset, ring_dist.orientation.to_quat(), 0.004, Color::GREEN); + draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, palm_negation, offset); let ring_tip = hand_pose[HandJoint::RING_TIP]; - gizmos.sphere(ring_tip.position.to_vec3() + offset, ring_tip.orientation.to_quat(), 0.002, Color::GREEN); + draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, palm_negation, offset); let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; - gizmos.sphere(little_meta.position.to_vec3() + offset, little_meta.orientation.to_quat(), 0.01, Color::BLUE); + draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, palm_negation, offset); let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; - gizmos.sphere(little_prox.position.to_vec3() + offset, little_prox.orientation.to_quat(), 0.008, Color::BLUE); + draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, palm_negation, offset); let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; - gizmos.sphere(little_inter.position.to_vec3() + offset, little_inter.orientation.to_quat(), 0.006, Color::BLUE); + draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, palm_negation, offset); let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; - gizmos.sphere(little_dist.position.to_vec3() + offset, little_dist.orientation.to_quat(), 0.004, Color::BLUE); + draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, palm_negation, offset); let little_tip = hand_pose[HandJoint::LITTLE_TIP]; - gizmos.sphere(little_tip.position.to_vec3() + offset, little_tip.orientation.to_quat(), 0.002, Color::BLUE); - - - + draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, palm_negation, offset); +} +fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, palm_negation: Vec3, offset: Vec3) { + gizmos.sphere(controller_backwards.mul_vec3(joint_pos + palm_negation) + offset, joint_rot, radius, color); } From c49f716a02c59508263bd3d43e5ebf021bf81b34 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 3 Oct 2023 15:03:34 -0400 Subject: [PATCH 14/66] normalized hand poses to palm --- examples/xr.rs | 157 +++++++++++++++++++++++++++++++------------------ 1 file changed, 99 insertions(+), 58 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 1f3df09..b1305da 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -82,117 +82,158 @@ fn draw_skeleton_hand(mut commands: Commands, let right_transform = right_controller_query.get_single().unwrap().0; let right_translation = right_transform.compute_transform().translation; let right_quat = right_transform.compute_transform().rotation; - gizmos.sphere(right_translation, Quat::IDENTITY, 0.01, Color::PINK); //we need to flip this i dont know why let flip = Quat::from_rotation_x(PI); let flap = Quat::from_rotation_z(-45.0*(PI/180.0)); - let controller_forward = right_quat.mul_quat(flip).mul_vec3(Vec3::Y); let controller_backward = right_quat.mul_quat(flip).mul_quat(flap); - gizmos.ray(right_translation, controller_forward, Color::PINK); let hand_pose: [Posef; 26] = [ - Posef { position: Vector3f {x: -0.548, y: -0.161, z: -0.137}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, //palm - Posef { position: Vector3f {x: -0.548, y: 0.161, z: -0.137}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, - Posef { position: Vector3f {x: -0.529, y: -0.198, z: -0.126}, orientation: Quaternionf {x: -0.744, y: -0.530, z: 0.156, w: -0.376}}, - Posef { position: Vector3f {x: -0.533, y: -0.175, z: -0.090}, orientation: Quaternionf {x: -0.786, y: -0.550, z: 0.126, w: -0.254}}, - Posef { position: Vector3f {x: -0.544, y: -0.158, z: -0.069}, orientation: Quaternionf {x: -0.729, y: -0.564, z: 0.027, w: -0.387}}, - Posef { position: Vector3f {x: -0.557, y: -0.150, z: -0.065}, orientation: Quaternionf {x: -0.585, y: -0.548, z: -0.140,w: -0.582}}, - Posef { position: Vector3f {x: -0.521, y: -0.182, z: -0.136}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317, w: -0.376}}, - Posef { position: Vector3f {x: -0.550, y: -0.135, z: -0.102}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317, w: -0.376}}, - Posef { position: Vector3f {x: -0.571, y: -0.112, z: -0.082}, orientation: Quaternionf {x: -0.244, y: -0.843, z: 0.256, w: -0.404}}, - Posef { position: Vector3f {x: -0.585, y: -0.102, z: -0.070}, orientation: Quaternionf {x: -0.200, y: -0.866, z: 0.165, w: -0.428}}, - Posef { position: Vector3f {x: -0.593, y: -0.098, z: -0.064}, orientation: Quaternionf {x: -0.172, y: -0.874, z: 0.110, w: -0.440}}, - Posef { position: Vector3f {x: -0.527, y: -0.178, z: -0.144}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370, w: -0.401}}, - Posef { position: Vector3f {x: -0.559, y: -0.132, z: -0.119}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370, w: -0.401}}, - Posef { position: Vector3f {x: -0.582, y: -0.101, z: -0.104}, orientation: Quaternionf {x: -0.175, y: -0.809, z: 0.371, w: -0.420}}, - Posef { position: Vector3f {x: -0.599, y: -0.089, z: -0.092}, orientation: Quaternionf {x: -0.109, y: -0.856, z: 0.245, w: -0.443}}, - Posef { position: Vector3f {x: -0.608, y: -0.084, z: -0.086}, orientation: Quaternionf {x: -0.075, y: -0.871, z: 0.180, w: -0.450}}, - Posef { position: Vector3f {x: -0.535, y: -0.178, z: -0.152}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408, w: -0.445}}, - Posef { position: Vector3f {x: -0.568, y: -0.136, z: -0.137}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408, w: -0.445}}, - Posef { position: Vector3f {x: -0.590, y: -0.106, z: -0.130}, orientation: Quaternionf {x: -0.131, y: -0.762, z: 0.432, w: -0.464}}, - Posef { position: Vector3f {x: -0.607, y: -0.092, z: -0.122}, orientation: Quaternionf {x: -0.071, y: -0.810, z: 0.332, w: -0.477}}, - Posef { position: Vector3f {x: -0.617, y: -0.086, z: -0.117}, orientation: Quaternionf {x: -0.029, y: -0.836, z: 0.260, w: -0.482}}, - Posef { position: Vector3f {x: -0.544, y: -0.183, z: -0.159}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481, w: -0.452}}, - Posef { position: Vector3f {x: -0.576, y: -0.143, z: -0.152}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481, w: -0.452}}, - Posef { position: Vector3f {x: -0.594, y: -0.119, z: -0.154}, orientation: Quaternionf {x: -0.061, y: -0.684, z: 0.534, w: -0.493}}, - Posef { position: Vector3f {x: -0.607, y: -0.108, z: -0.152}, orientation: Quaternionf {x: 0.002, y: -0.745, z: 0.444, w: -0.498}}, - Posef { position: Vector3f {x: -0.616, y: -0.102, z: -0.150}, orientation: Quaternionf {x: 0.045, y: -0.780, z: 0.378, w: -0.496}}, + 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.0, y: 0.32, z: 0.0}, 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}}, ]; + //log_hand(hand_pose); + + //cursed wrist math let wrist_dist = Vec3{ x: -0.01, y: -0.040, z: -0.015}; //cursed offset - let palm_negation = Vec3 { x: 0.548, y: 0.161, z: 0.137 }; let offset = right_translation; //old stuff dont touch for now let palm = hand_pose[HandJoint::PALM]; - gizmos.sphere(palm.position.to_vec3() + offset + palm_negation, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); + gizmos.sphere(palm.position.to_vec3() + offset, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); let rotated_wfp = palm.position.to_vec3() + right_quat.mul_quat(flip).mul_vec3(wrist_dist); - gizmos.sphere(offset + palm_negation + rotated_wfp, palm.orientation.to_quat(), 0.01, Color::GRAY); + gizmos.sphere(offset + rotated_wfp, palm.orientation.to_quat(), 0.01, Color::GRAY); let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; - draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, offset); let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; - draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, offset); let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; - draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, offset); let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; - draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, offset); let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; - draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, offset); let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; - draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, offset); let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; - draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, offset); let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; - draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, offset); let index_tip = hand_pose[HandJoint::INDEX_TIP]; - draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, offset); let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; - draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, offset); let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; - draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, offset); let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; - draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, offset); let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; - draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, offset); let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; - draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, offset); let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; - draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, offset); let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; - draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, offset); let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; - draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, offset); let ring_dist = hand_pose[HandJoint::RING_DISTAL]; - draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, offset); let ring_tip = hand_pose[HandJoint::RING_TIP]; - draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, offset); let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; - draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, offset); let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; - draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, offset); let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; - draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, offset); let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; - draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, offset); let little_tip = hand_pose[HandJoint::LITTLE_TIP]; - draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, offset); + + } -fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, palm_negation: Vec3, offset: Vec3) { - gizmos.sphere(controller_backwards.mul_vec3(joint_pos + palm_negation) + offset, joint_rot, radius, color); +fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, offset: Vec3) { + gizmos.sphere(controller_backwards.mul_vec3(joint_pos) + offset, joint_rot, radius, color); +} + +fn log_hand(hand_pose: [Posef; 26]) { + let palm_vec = hand_pose[HandJoint::PALM].position.to_vec3(); + info!("palm: {}", hand_pose[HandJoint::PALM].position.to_vec3() - palm_vec); + info!("wrist: {}", hand_pose[HandJoint::WRIST].position.to_vec3() - palm_vec); + + info!("tm: {}", hand_pose[HandJoint::THUMB_METACARPAL].position.to_vec3() - palm_vec); + info!("tp: {}", hand_pose[HandJoint::THUMB_PROXIMAL].position.to_vec3() - palm_vec); + info!("td: {}", hand_pose[HandJoint::THUMB_DISTAL].position.to_vec3() - palm_vec); + info!("tt: {}", hand_pose[HandJoint::THUMB_TIP].position.to_vec3() - palm_vec); + + info!("im: {}", hand_pose[HandJoint::INDEX_METACARPAL].position.to_vec3() - palm_vec); + info!("ip: {}", hand_pose[HandJoint::INDEX_PROXIMAL].position.to_vec3() - palm_vec); + info!("ii: {}", hand_pose[HandJoint::INDEX_INTERMEDIATE].position.to_vec3() - palm_vec); + info!("id: {}", hand_pose[HandJoint::INDEX_DISTAL].position.to_vec3() - palm_vec); + info!("it: {}", hand_pose[HandJoint::INDEX_TIP].position.to_vec3() - palm_vec); + + info!("mm: {}", hand_pose[HandJoint::MIDDLE_METACARPAL].position.to_vec3() - palm_vec); + info!("mp: {}", hand_pose[HandJoint::MIDDLE_PROXIMAL].position.to_vec3() - palm_vec); + info!("mi: {}", hand_pose[HandJoint::MIDDLE_INTERMEDIATE].position.to_vec3() - palm_vec); + info!("md: {}", hand_pose[HandJoint::MIDDLE_DISTAL].position.to_vec3() - palm_vec); + info!("mt: {}", hand_pose[HandJoint::MIDDLE_TIP].position.to_vec3() - palm_vec); + + info!("rm: {}", hand_pose[HandJoint::RING_METACARPAL].position.to_vec3() - palm_vec); + info!("rp: {}", hand_pose[HandJoint::RING_PROXIMAL].position.to_vec3() - palm_vec); + info!("ri: {}", hand_pose[HandJoint::RING_INTERMEDIATE].position.to_vec3() - palm_vec); + info!("rd: {}", hand_pose[HandJoint::RING_DISTAL].position.to_vec3() - palm_vec); + info!("rt: {}", hand_pose[HandJoint::RING_TIP].position.to_vec3() - palm_vec); + + info!("lm: {}", hand_pose[HandJoint::LITTLE_METACARPAL].position.to_vec3() - palm_vec); + info!("lp: {}", hand_pose[HandJoint::LITTLE_PROXIMAL].position.to_vec3() - palm_vec); + info!("li: {}", hand_pose[HandJoint::LITTLE_INTERMEDIATE].position.to_vec3() - palm_vec); + info!("ld: {}", hand_pose[HandJoint::LITTLE_DISTAL].position.to_vec3() - palm_vec); + info!("lt: {}", hand_pose[HandJoint::LITTLE_TIP].position.to_vec3() - palm_vec); } From 68cdf193904edcbfed3437fc851d45e2f9ed9ef9 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 3 Oct 2023 15:47:18 -0400 Subject: [PATCH 15/66] both hands work --- examples/xr.rs | 117 ++++++++++++++++++++++++++++--------------------- 1 file changed, 68 insertions(+), 49 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index b1305da..3d8058b 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,9 +1,10 @@ use std::f32::consts::PI; +use std::ops::Mul; use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; -use bevy_openxr::xr_input::{Vec3Conv, QuatConv}; +use bevy_openxr::xr_input::{Vec3Conv, QuatConv, Hand}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ @@ -24,7 +25,7 @@ fn main() { .add_systems(Startup, setup) .add_systems(Update, proto_locomotion) .add_systems(Startup, spawn_controllers_example) - .add_systems(Update, draw_skeleton_hand) + .add_systems(Update, draw_skeleton_hands) .insert_resource(PrototypeLocomotionConfig::default()) .run(); } @@ -72,24 +73,39 @@ fn setup( },)); } -fn draw_skeleton_hand(mut commands: Commands, +fn draw_skeleton_hands(mut commands: Commands, mut gizmos: Gizmos, right_controller_query: Query<( &GlobalTransform, With, -)>, ) { +)>, +left_controller_query: Query<( + &GlobalTransform, + With, +)>,) { + + let left_hand_transform = left_controller_query.get_single().unwrap().0.compute_transform(); + draw_hand(&mut gizmos, left_hand_transform, Hand::Left); + let right_hand_transform = right_controller_query.get_single().unwrap().0.compute_transform(); + draw_hand(&mut gizmos, right_hand_transform, Hand::Right) + + + +} + +fn draw_hand(mut gizmos: &mut Gizmos, + controller_transform: Transform, +hand: Hand) { //draw debug for controller grip center to match palm to - let right_transform = right_controller_query.get_single().unwrap().0; - let right_translation = right_transform.compute_transform().translation; - let right_quat = right_transform.compute_transform().rotation; + let hand_translation = controller_transform.translation; + let hand_quat = controller_transform.rotation; //we need to flip this i dont know why let flip = Quat::from_rotation_x(PI); - let flap = Quat::from_rotation_z(-45.0*(PI/180.0)); - let controller_backward = right_quat.mul_quat(flip).mul_quat(flap); + let controller_backward = hand_quat.mul_quat(flip); - let hand_pose: [Posef; 26] = [ + let test: [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.0, y: 0.32, z: 0.0}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, + 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}}, @@ -121,80 +137,83 @@ fn draw_skeleton_hand(mut commands: Commands, 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}}, ]; + let hand_pose = flip_hand_pose(test.clone(), hand); + //log_hand(hand_pose); - - //cursed wrist math - let wrist_dist = Vec3{ x: -0.01, y: -0.040, z: -0.015}; - - //cursed offset - let offset = right_translation; - - //old stuff dont touch for now let palm = hand_pose[HandJoint::PALM]; - gizmos.sphere(palm.position.to_vec3() + offset, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); + gizmos.sphere(palm.position.to_vec3() + hand_translation, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); - let rotated_wfp = palm.position.to_vec3() + right_quat.mul_quat(flip).mul_vec3(wrist_dist); - gizmos.sphere(offset + rotated_wfp, palm.orientation.to_quat(), 0.01, Color::GRAY); + let wrist = hand_pose[HandJoint::WRIST]; + draw_joint(&mut gizmos, wrist.position.to_vec3(), wrist.orientation.to_quat(), 0.01, Color::GRAY, controller_backward, hand_translation); let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; - draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, offset); + draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, hand_translation); let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; - draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, offset); + draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, hand_translation); let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; - draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, offset); + draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, hand_translation); let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; - draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, offset); + draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, hand_translation); let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; - draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, hand_translation); let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; - draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, hand_translation); let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; - draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, hand_translation); let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; - draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, hand_translation); let index_tip = hand_pose[HandJoint::INDEX_TIP]; - draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, hand_translation); let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; - draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, hand_translation); let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; - draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, hand_translation); let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; - draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, hand_translation); let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; - draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, hand_translation); let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; - draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, hand_translation); let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; - draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, hand_translation); let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; - draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, hand_translation); let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; - draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, hand_translation); let ring_dist = hand_pose[HandJoint::RING_DISTAL]; - draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, hand_translation); let ring_tip = hand_pose[HandJoint::RING_TIP]; - draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, hand_translation); let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; - draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, offset); + draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, hand_translation); let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; - draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, offset); + draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, hand_translation); let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; - draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, offset); + draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, hand_translation); let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; - draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, offset); + draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, hand_translation); let little_tip = hand_pose[HandJoint::LITTLE_TIP]; - draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, offset); - - - + draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, hand_translation); +} +fn flip_hand_pose(hand_pose: [Posef; 26], hand: Hand) -> [Posef; 26] { + let mut new_pose = hand_pose; + match hand { + Hand::Left => { + for pose in new_pose.iter_mut() { + pose.position.x = -pose.position.x; + } + }, + Hand::Right => (), + } + return new_pose; } fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, offset: Vec3) { From 5818e57d4151fb7a169f8bc929d9ae74865b6d15 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sat, 7 Oct 2023 17:05:47 -0400 Subject: [PATCH 16/66] left hand works too --- examples/xr.rs | 844 ++++++++++++++++++++++++++++++------- src/xr_input/hand_poses.rs | 520 +++++++++++++++++++++++ src/xr_input/mod.rs | 1 + 3 files changed, 1224 insertions(+), 141 deletions(-) create mode 100644 src/xr_input/hand_poses.rs diff --git a/examples/xr.rs b/examples/xr.rs index 3d8058b..d34c4b7 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -2,16 +2,18 @@ use std::f32::consts::PI; use std::ops::Mul; use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; -use bevy::prelude::*; +use bevy::math::vec3; use bevy::transform::components::Transform; -use bevy_openxr::xr_input::{Vec3Conv, QuatConv, Hand}; +use bevy::{gizmos, prelude::*}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; +use bevy_openxr::xr_input::hand_poses::*; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, }; +use bevy_openxr::xr_input::{Hand, QuatConv, Vec3Conv}; use bevy_openxr::DefaultXrPlugins; -use openxr::{Posef, Quaternionf, Vector3f, HandJoint}; +use openxr::{HandJoint, Posef, Quaternionf, Vector3f}; fn main() { color_eyre::install().unwrap(); @@ -68,34 +70,306 @@ fn setup( }); // camera commands.spawn((Camera3dBundle { - transform: Transform::from_xyz(0.25, 1.25, 0.0).looking_at(Vec3{x: -0.548, y: -0.161, z: -0.137}, 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() },)); } -fn draw_skeleton_hands(mut commands: Commands, +fn draw_skeleton_hands( + mut commands: Commands, mut gizmos: Gizmos, - right_controller_query: Query<( - &GlobalTransform, - With, -)>, -left_controller_query: Query<( - &GlobalTransform, - With, -)>,) { - - let left_hand_transform = left_controller_query.get_single().unwrap().0.compute_transform(); - draw_hand(&mut gizmos, left_hand_transform, Hand::Left); - let right_hand_transform = right_controller_query.get_single().unwrap().0.compute_transform(); - draw_hand(&mut gizmos, right_hand_transform, Hand::Right) - - - + right_controller_query: Query<(&GlobalTransform, With)>, + left_controller_query: Query<(&GlobalTransform, With)>, +) { + let left_hand_transform = left_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left); + let right_hand_transform = right_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); + draw_hand_bones(&mut gizmos, right_hand_transform, Hand::Right); } -fn draw_hand(mut gizmos: &mut Gizmos, - controller_transform: Transform, -hand: Hand) { +fn pose_array_to_transform_array(hand_pose: [Posef; 26]) -> [Transform; 26] { + let mut result_array: [Transform; 26] = [Transform::default(); 26]; + for (place, data) in result_array.iter_mut().zip(hand_pose.iter()) { + *place = Transform { + translation: data.position.to_vec3(), + rotation: data.orientation.to_quat(), + scale: Vec3::splat(1.0), + } + } + return result_array; +} + +fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: 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, + }; + //get paml 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); + //draw debug rays + gizmos.ray( + hand_translation, + palm_quat.mul_vec3(Vec3::Z * 0.2), + Color::BLUE, + ); + gizmos.ray( + hand_translation, + palm_quat.mul_vec3(Vec3::Y * 0.2), + Color::GREEN, + ); + gizmos.ray( + hand_translation, + palm_quat.mul_vec3(Vec3::X * 0.2), + Color::RED, + ); + //get simulated bones + let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); + //draw controller-palm bone(should be zero length) + let palm = hand_transform_array[HandJoint::PALM]; + gizmos.ray(hand_translation, palm.translation, Color::WHITE); + //draw palm-wrist + let wrist = hand_transform_array[HandJoint::WRIST]; + gizmos.ray( + hand_translation + palm.translation, + palm_quat.mul_vec3(wrist.translation), + Color::GRAY, + ); + + //thumb + //better finger drawing? + 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 color = Color::RED; + let splay = Quat::from_rotation_y(splay_direction * 30.0 * PI / 180.0); + let splay_quat = palm_quat.mul_quat(splay); + for bone in thumb_joints.iter() { + match prior_start { + Some(start) => { + let tp_lrot = Quat::from_rotation_y(splay_direction * 5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } + + //better finger drawing? + 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 color = Color::ORANGE; + 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 tp_lrot = Quat::from_rotation_x(-5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } + + //better finger drawing? + 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 color = Color::YELLOW; + 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 tp_lrot = Quat::from_rotation_x(-5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } + //better finger drawing? + 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 color = Color::GREEN; + 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 tp_lrot = Quat::from_rotation_x(-5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } + + //better finger drawing? + 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 color = Color::BLUE; + 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 tp_lrot = Quat::from_rotation_x(-5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } +} + +fn draw_hand(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: Hand) { //draw debug for controller grip center to match palm to let hand_translation = controller_transform.translation; let hand_quat = controller_transform.rotation; @@ -103,104 +377,280 @@ hand: Hand) { let flip = Quat::from_rotation_x(PI); let controller_backward = hand_quat.mul_quat(flip); - let test: [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}}, + let test_hand_pose = get_test_hand_pose_array(); - 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}}, + let hand_pose = flip_hand_pose(test_hand_pose.clone(), hand); - 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}}, + // let hand_transform_array: [Transform; 26] = pose_array_to_transform_array(hand_pose); + let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); - 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}}, + let palm = hand_transform_array[HandJoint::PALM]; + gizmos.sphere( + palm.translation + hand_translation, + palm.rotation.mul_quat(controller_backward), + 0.01, + Color::WHITE, + ); - 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}}, + let wrist = hand_transform_array[HandJoint::WRIST]; + draw_joint( + &mut gizmos, + wrist.translation, + wrist.rotation, + 0.01, + Color::GRAY, + controller_backward, + hand_translation, + ); - 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}}, - ]; + let thumb_meta = hand_transform_array[HandJoint::THUMB_METACARPAL]; + draw_joint( + &mut gizmos, + thumb_meta.translation, + thumb_meta.rotation, + 0.01, + Color::RED, + controller_backward, + hand_translation, + ); - let hand_pose = flip_hand_pose(test.clone(), hand); + let thumb_prox = hand_transform_array[HandJoint::THUMB_PROXIMAL]; + draw_joint( + &mut gizmos, + thumb_prox.translation, + thumb_prox.rotation, + 0.008, + Color::RED, + controller_backward, + hand_translation, + ); + let thumb_dist = hand_transform_array[HandJoint::THUMB_DISTAL]; + draw_joint( + &mut gizmos, + thumb_dist.translation, + thumb_dist.rotation, + 0.006, + Color::RED, + controller_backward, + hand_translation, + ); + let thumb_tip = hand_transform_array[HandJoint::THUMB_TIP]; + draw_joint( + &mut gizmos, + thumb_tip.translation, + thumb_tip.rotation, + 0.004, + Color::RED, + controller_backward, + hand_translation, + ); - //log_hand(hand_pose); + let index_meta = hand_transform_array[HandJoint::INDEX_METACARPAL]; + draw_joint( + &mut gizmos, + index_meta.translation, + index_meta.rotation, + 0.01, + Color::ORANGE, + controller_backward, + hand_translation, + ); + let index_prox = hand_transform_array[HandJoint::INDEX_PROXIMAL]; + draw_joint( + &mut gizmos, + index_prox.translation, + index_prox.rotation, + 0.008, + Color::ORANGE, + controller_backward, + hand_translation, + ); - let palm = hand_pose[HandJoint::PALM]; - gizmos.sphere(palm.position.to_vec3() + hand_translation, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); + let index_inter = hand_transform_array[HandJoint::INDEX_INTERMEDIATE]; + draw_joint( + &mut gizmos, + index_inter.translation, + index_inter.rotation, + 0.006, + Color::ORANGE, + controller_backward, + hand_translation, + ); - let wrist = hand_pose[HandJoint::WRIST]; - draw_joint(&mut gizmos, wrist.position.to_vec3(), wrist.orientation.to_quat(), 0.01, Color::GRAY, controller_backward, hand_translation); + let index_dist = hand_transform_array[HandJoint::INDEX_DISTAL]; + draw_joint( + &mut gizmos, + index_dist.translation, + index_dist.rotation, + 0.004, + Color::ORANGE, + controller_backward, + hand_translation, + ); + let index_tip = hand_transform_array[HandJoint::INDEX_TIP]; + draw_joint( + &mut gizmos, + index_tip.translation, + index_tip.rotation, + 0.002, + Color::ORANGE, + controller_backward, + hand_translation, + ); - let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; - draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, hand_translation); + let middle_meta = hand_transform_array[HandJoint::MIDDLE_METACARPAL]; + draw_joint( + &mut gizmos, + middle_meta.translation, + middle_meta.rotation, + 0.01, + Color::YELLOW, + controller_backward, + hand_translation, + ); + let middle_prox = hand_transform_array[HandJoint::MIDDLE_PROXIMAL]; + draw_joint( + &mut gizmos, + middle_prox.translation, + middle_prox.rotation, + 0.008, + Color::YELLOW, + controller_backward, + hand_translation, + ); + let middle_inter = hand_transform_array[HandJoint::MIDDLE_INTERMEDIATE]; + draw_joint( + &mut gizmos, + middle_inter.translation, + middle_inter.rotation, + 0.006, + Color::YELLOW, + controller_backward, + hand_translation, + ); + let middle_dist = hand_transform_array[HandJoint::MIDDLE_DISTAL]; + draw_joint( + &mut gizmos, + middle_dist.translation, + middle_dist.rotation, + 0.004, + Color::YELLOW, + controller_backward, + hand_translation, + ); + let middle_tip = hand_transform_array[HandJoint::MIDDLE_TIP]; + draw_joint( + &mut gizmos, + middle_tip.translation, + middle_tip.rotation, + 0.002, + Color::YELLOW, + controller_backward, + hand_translation, + ); - let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; - draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, hand_translation); - let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; - draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, hand_translation); - let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; - draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, hand_translation); + let ring_meta = hand_transform_array[HandJoint::RING_METACARPAL]; + draw_joint( + &mut gizmos, + ring_meta.translation, + ring_meta.rotation, + 0.01, + Color::GREEN, + controller_backward, + hand_translation, + ); + let ring_prox = hand_transform_array[HandJoint::RING_PROXIMAL]; + draw_joint( + &mut gizmos, + ring_prox.translation, + ring_prox.rotation, + 0.008, + Color::GREEN, + controller_backward, + hand_translation, + ); + let ring_inter = hand_transform_array[HandJoint::RING_INTERMEDIATE]; + draw_joint( + &mut gizmos, + ring_inter.translation, + ring_inter.rotation, + 0.006, + Color::GREEN, + controller_backward, + hand_translation, + ); + let ring_dist = hand_transform_array[HandJoint::RING_DISTAL]; + draw_joint( + &mut gizmos, + ring_dist.translation, + ring_dist.rotation, + 0.004, + Color::GREEN, + controller_backward, + hand_translation, + ); + let ring_tip = hand_transform_array[HandJoint::RING_TIP]; + draw_joint( + &mut gizmos, + ring_tip.translation, + ring_tip.rotation, + 0.002, + Color::GREEN, + controller_backward, + hand_translation, + ); - let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; - draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, hand_translation); - let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; - draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, hand_translation); - let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; - draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, hand_translation); - let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; - draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, hand_translation); - let index_tip = hand_pose[HandJoint::INDEX_TIP]; - draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, hand_translation); - - let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; - draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, hand_translation); - let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; - draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, hand_translation); - let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; - draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, hand_translation); - let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; - draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, hand_translation); - let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; - draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, hand_translation); - - let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; - draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, hand_translation); - let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; - draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, hand_translation); - let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; - draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, hand_translation); - let ring_dist = hand_pose[HandJoint::RING_DISTAL]; - draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, hand_translation); - let ring_tip = hand_pose[HandJoint::RING_TIP]; - draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, hand_translation); - - let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; - draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, hand_translation); - let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; - draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, hand_translation); - let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; - draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, hand_translation); - let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; - draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, hand_translation); - let little_tip = hand_pose[HandJoint::LITTLE_TIP]; - draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, hand_translation); + let little_meta = hand_transform_array[HandJoint::LITTLE_METACARPAL]; + draw_joint( + &mut gizmos, + little_meta.translation, + little_meta.rotation, + 0.01, + Color::BLUE, + controller_backward, + hand_translation, + ); + let little_prox = hand_transform_array[HandJoint::LITTLE_PROXIMAL]; + draw_joint( + &mut gizmos, + little_prox.translation, + little_prox.rotation, + 0.008, + Color::BLUE, + controller_backward, + hand_translation, + ); + let little_inter = hand_transform_array[HandJoint::LITTLE_INTERMEDIATE]; + draw_joint( + &mut gizmos, + little_inter.translation, + little_inter.rotation, + 0.006, + Color::BLUE, + controller_backward, + hand_translation, + ); + let little_dist = hand_transform_array[HandJoint::LITTLE_DISTAL]; + draw_joint( + &mut gizmos, + little_dist.translation, + little_dist.rotation, + 0.004, + Color::BLUE, + controller_backward, + hand_translation, + ); + let little_tip = hand_transform_array[HandJoint::LITTLE_TIP]; + draw_joint( + &mut gizmos, + little_tip.translation, + little_tip.rotation, + 0.002, + Color::BLUE, + controller_backward, + hand_translation, + ); } fn flip_hand_pose(hand_pose: [Posef; 26], hand: Hand) -> [Posef; 26] { @@ -210,52 +660,164 @@ fn flip_hand_pose(hand_pose: [Posef; 26], hand: Hand) -> [Posef; 26] { for pose in new_pose.iter_mut() { pose.position.x = -pose.position.x; } - }, + } Hand::Right => (), } return new_pose; } -fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, offset: Vec3) { - gizmos.sphere(controller_backwards.mul_vec3(joint_pos) + offset, joint_rot, radius, color); +fn draw_joint( + gizmos: &mut Gizmos, + joint_pos: Vec3, + joint_rot: Quat, + radius: f32, + color: Color, + controller_backwards: Quat, + offset: Vec3, +) { + gizmos.sphere( + controller_backwards.mul_vec3(joint_pos) + offset, + joint_rot, + radius, + color, + ); } fn log_hand(hand_pose: [Posef; 26]) { - let palm_vec = hand_pose[HandJoint::PALM].position.to_vec3(); - info!("palm: {}", hand_pose[HandJoint::PALM].position.to_vec3() - palm_vec); - info!("wrist: {}", hand_pose[HandJoint::WRIST].position.to_vec3() - palm_vec); + 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!("tm: {}", hand_pose[HandJoint::THUMB_METACARPAL].position.to_vec3() - palm_vec); - info!("tp: {}", hand_pose[HandJoint::THUMB_PROXIMAL].position.to_vec3() - palm_vec); - info!("td: {}", hand_pose[HandJoint::THUMB_DISTAL].position.to_vec3() - palm_vec); - info!("tt: {}", hand_pose[HandJoint::THUMB_TIP].position.to_vec3() - palm_vec); - - info!("im: {}", hand_pose[HandJoint::INDEX_METACARPAL].position.to_vec3() - palm_vec); - info!("ip: {}", hand_pose[HandJoint::INDEX_PROXIMAL].position.to_vec3() - palm_vec); - info!("ii: {}", hand_pose[HandJoint::INDEX_INTERMEDIATE].position.to_vec3() - palm_vec); - info!("id: {}", hand_pose[HandJoint::INDEX_DISTAL].position.to_vec3() - palm_vec); - info!("it: {}", hand_pose[HandJoint::INDEX_TIP].position.to_vec3() - palm_vec); + 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!("mm: {}", hand_pose[HandJoint::MIDDLE_METACARPAL].position.to_vec3() - palm_vec); - info!("mp: {}", hand_pose[HandJoint::MIDDLE_PROXIMAL].position.to_vec3() - palm_vec); - info!("mi: {}", hand_pose[HandJoint::MIDDLE_INTERMEDIATE].position.to_vec3() - palm_vec); - info!("md: {}", hand_pose[HandJoint::MIDDLE_DISTAL].position.to_vec3() - palm_vec); - info!("mt: {}", hand_pose[HandJoint::MIDDLE_TIP].position.to_vec3() - palm_vec); + 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!("rm: {}", hand_pose[HandJoint::RING_METACARPAL].position.to_vec3() - palm_vec); - info!("rp: {}", hand_pose[HandJoint::RING_PROXIMAL].position.to_vec3() - palm_vec); - info!("ri: {}", hand_pose[HandJoint::RING_INTERMEDIATE].position.to_vec3() - palm_vec); - info!("rd: {}", hand_pose[HandJoint::RING_DISTAL].position.to_vec3() - palm_vec); - info!("rt: {}", hand_pose[HandJoint::RING_TIP].position.to_vec3() - palm_vec); + 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!("lm: {}", hand_pose[HandJoint::LITTLE_METACARPAL].position.to_vec3() - palm_vec); - info!("lp: {}", hand_pose[HandJoint::LITTLE_PROXIMAL].position.to_vec3() - palm_vec); - info!("li: {}", hand_pose[HandJoint::LITTLE_INTERMEDIATE].position.to_vec3() - palm_vec); - info!("ld: {}", hand_pose[HandJoint::LITTLE_DISTAL].position.to_vec3() - palm_vec); - info!("lt: {}", hand_pose[HandJoint::LITTLE_TIP].position.to_vec3() - palm_vec); + 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() + ); } - fn spawn_controllers_example(mut commands: Commands) { //left hand commands.spawn(( 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/mod.rs b/src/xr_input/mod.rs index 55e7c56..1ee3e71 100644 --- a/src/xr_input/mod.rs +++ b/src/xr_input/mod.rs @@ -4,6 +4,7 @@ pub mod oculus_touch; pub mod prototype_locomotion; pub mod trackers; pub mod xr_camera; +pub mod hand_poses; use crate::resources::XrSession; use crate::xr_begin_frame; From 319a2dcf215975679d02216d115eb350f8ccd081 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sat, 7 Oct 2023 18:38:25 -0400 Subject: [PATCH 17/66] hand state resource is used to drive skeleton --- examples/xr.rs | 318 +++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 283 insertions(+), 35 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index d34c4b7..7f65bc4 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -5,8 +5,11 @@ use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::math::vec3; use bevy::transform::components::Transform; use bevy::{gizmos, prelude::*}; +use bevy_openxr::input::XrInput; +use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; use bevy_openxr::xr_input::hand_poses::*; +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, @@ -28,7 +31,9 @@ fn main() { .add_systems(Update, proto_locomotion) .add_systems(Startup, spawn_controllers_example) .add_systems(Update, draw_skeleton_hands) + .add_systems(PreUpdate, update_hand_states) .insert_resource(PrototypeLocomotionConfig::default()) + .insert_resource(HandStatesResource::default()) .run(); } @@ -82,25 +87,144 @@ fn setup( },)); } +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"), + } +} + fn draw_skeleton_hands( mut commands: Commands, mut gizmos: Gizmos, right_controller_query: Query<(&GlobalTransform, With)>, left_controller_query: Query<(&GlobalTransform, With)>, + hand_states_option: Option>, ) { - let left_hand_transform = left_controller_query - .get_single() - .unwrap() - .0 - .compute_transform(); - draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left); - let right_hand_transform = right_controller_query - .get_single() - .unwrap() - .0 - .compute_transform(); - // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); - draw_hand_bones(&mut gizmos, right_hand_transform, Hand::Right); + match hand_states_option { + Some(hands) => { + let left_hand_transform = left_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left, hands.left); + let right_hand_transform = right_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); + draw_hand_bones(&mut gizmos, right_hand_transform, Hand::Right, hands.right); + } + None => info!("hand states resource not initialized yet"), + } } fn pose_array_to_transform_array(hand_pose: [Posef; 26]) -> [Transform; 26] { @@ -114,8 +238,108 @@ fn pose_array_to_transform_array(hand_pose: [Posef; 26]) -> [Transform; 26] { } return result_array; } +#[derive(Clone, Copy)] +pub enum ButtonState { + OFF, + TOUCHED, + PRESSED, +} -fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: Hand) { +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 { + grip: f32, + trigger_state: TriggerState, + a_button: ButtonState, + b_button: ButtonState, + thumbstick: ThumbstickState, +} + +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(), + } + } +} + +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; + } +} + +fn draw_hand_bones( + mut gizmos: &mut Gizmos, + controller_transform: Transform, + hand: Hand, + hand_state: HandState, +) { let left_hand_rot = Quat::from_rotation_y(180.0 * PI / 180.0); let hand_translation: Vec3 = match hand { Hand::Left => controller_transform.translation, @@ -131,26 +355,30 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han Hand::Left => -1.0, Hand::Right => 1.0, }; + //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 paml 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); //draw debug rays - gizmos.ray( - hand_translation, - palm_quat.mul_vec3(Vec3::Z * 0.2), - Color::BLUE, - ); - gizmos.ray( - hand_translation, - palm_quat.mul_vec3(Vec3::Y * 0.2), - Color::GREEN, - ); - gizmos.ray( - hand_translation, - palm_quat.mul_vec3(Vec3::X * 0.2), - Color::RED, - ); + // gizmos.ray( + // hand_translation, + // palm_quat.mul_vec3(Vec3::Z * 0.2), + // Color::BLUE, + // ); + // gizmos.ray( + // hand_translation, + // palm_quat.mul_vec3(Vec3::Y * 0.2), + // Color::GREEN, + // ); + // gizmos.ray( + // hand_translation, + // palm_quat.mul_vec3(Vec3::X * 0.2), + // Color::RED, + // ); //get simulated bones let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); //draw controller-palm bone(should be zero length) @@ -177,11 +405,13 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han let mut prior_vector: Option = None; let color = Color::RED; let splay = Quat::from_rotation_y(splay_direction * 30.0 * PI / 180.0); - let splay_quat = palm_quat.mul_quat(splay); + 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 tp_lrot = Quat::from_rotation_y(splay_direction * 5.0 * PI / 180.0); + 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(); @@ -205,6 +435,7 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han } } + //index //better finger drawing? let thumb_joints = [ HandJoint::INDEX_METACARPAL, @@ -222,7 +453,8 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han for bone in thumb_joints.iter() { match prior_start { Some(start) => { - let tp_lrot = Quat::from_rotation_x(-5.0 * PI / 180.0); + 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(); @@ -263,7 +495,8 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han for bone in thumb_joints.iter() { match prior_start { Some(start) => { - let tp_lrot = Quat::from_rotation_x(-5.0 * PI / 180.0); + 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(); @@ -303,7 +536,8 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han for bone in thumb_joints.iter() { match prior_start { Some(start) => { - let tp_lrot = Quat::from_rotation_x(-5.0 * PI / 180.0); + 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(); @@ -344,7 +578,8 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han for bone in thumb_joints.iter() { match prior_start { Some(start) => { - let tp_lrot = Quat::from_rotation_x(-5.0 * PI / 180.0); + 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(); @@ -369,6 +604,19 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han } } +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 draw_hand(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: Hand) { //draw debug for controller grip center to match palm to let hand_translation = controller_transform.translation; From abfc376c9957267bc917a47651b54d83c4e4fb6a Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 00:11:23 -0400 Subject: [PATCH 18/66] spawn bones and update --- examples/xr.rs | 556 ++++++++++++++++++-------------------------- src/xr_input/mod.rs | 4 +- 2 files changed, 231 insertions(+), 329 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 7f65bc4..23de2bd 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -32,6 +32,8 @@ fn main() { .add_systems(Startup, spawn_controllers_example) .add_systems(Update, draw_skeleton_hands) .add_systems(PreUpdate, update_hand_states) + .add_systems(PostUpdate, draw_hand_entities) + .add_systems(Startup, spawn_hand_entities) .insert_resource(PrototypeLocomotionConfig::default()) .insert_resource(HandStatesResource::default()) .run(); @@ -87,6 +89,49 @@ fn setup( },)); } +pub fn spawn_hand_entities(mut commands: Commands) { + let hands = [Hand::Left, Hand::Right]; + let bones = [ + HandBone::PALM, + HandBone::WRIST, + HandBone::THUMB_METACARPAL, + HandBone::THUMB_PROXIMAL, + HandBone::THUMB_DISTAL, + HandBone::THUMB_TIP, + HandBone::INDEX_METACARPAL, + HandBone::INDEX_PROXIMAL, + HandBone::INDEX_INTERMEDIATE, + HandBone::INDEX_DISTAL, + HandBone::INDEX_TIP, + HandBone::MIDDLE_METACARPAL, + HandBone::MIDDLE_PROXIMAL, + HandBone::MIDDLE_INTERMEDIATE, + HandBone::MIDDLE_DISTAL, + HandBone::MIDDLE_TIP, + HandBone::RING_METACARPAL, + HandBone::RING_PROXIMAL, + HandBone::RING_INTERMEDIATE, + HandBone::RING_DISTAL, + HandBone::RING_TIP, + HandBone::LITTLE_METACARPAL, + HandBone::LITTLE_PROXIMAL, + HandBone::LITTLE_INTERMEDIATE, + HandBone::LITTLE_DISTAL, + HandBone::LITTLE_TIP, + ]; + + for hand in hands.iter() { + for bone in bones.iter() { + commands.spawn(( + SpatialBundle::default(), + bone.clone(), + OpenXRTracker, + hand.clone(), + )); + } + } +} + pub fn update_hand_states( oculus_controller: Res, hand_states_option: Option>, @@ -201,11 +246,11 @@ pub fn update_hand_states( } fn draw_skeleton_hands( - mut commands: Commands, mut gizmos: Gizmos, 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)>, ) { match hand_states_option { Some(hands) => { @@ -214,30 +259,61 @@ fn draw_skeleton_hands( .unwrap() .0 .compute_transform(); - draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left, hands.left); + draw_hand_bones( + &mut gizmos, + left_hand_transform, + Hand::Left, + hands.left, + &mut hand_bone_query, + ); let right_hand_transform = right_controller_query .get_single() .unwrap() .0 .compute_transform(); // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); - draw_hand_bones(&mut gizmos, right_hand_transform, Hand::Right, hands.right); + draw_hand_bones( + &mut gizmos, + right_hand_transform, + Hand::Right, + hands.right, + &mut hand_bone_query, + ); } None => info!("hand states resource not initialized yet"), } } -fn pose_array_to_transform_array(hand_pose: [Posef; 26]) -> [Transform; 26] { - let mut result_array: [Transform; 26] = [Transform::default(); 26]; - for (place, data) in result_array.iter_mut().zip(hand_pose.iter()) { - *place = Transform { - translation: data.position.to_vec3(), - rotation: data.orientation.to_quat(), - scale: Vec3::splat(1.0), - } - } - return result_array; +#[derive(Component, Debug, Clone, Copy)] +pub enum HandBone { + PALM, + WRIST, + THUMB_METACARPAL, + THUMB_PROXIMAL, + THUMB_DISTAL, + THUMB_TIP, + INDEX_METACARPAL, + INDEX_PROXIMAL, + INDEX_INTERMEDIATE, + INDEX_DISTAL, + INDEX_TIP, + MIDDLE_METACARPAL, + MIDDLE_PROXIMAL, + MIDDLE_INTERMEDIATE, + MIDDLE_DISTAL, + MIDDLE_TIP, + RING_METACARPAL, + RING_PROXIMAL, + RING_INTERMEDIATE, + RING_DISTAL, + RING_TIP, + LITTLE_METACARPAL, + LITTLE_PROXIMAL, + LITTLE_INTERMEDIATE, + LITTLE_DISTAL, + LITTLE_TIP, } + #[derive(Clone, Copy)] pub enum ButtonState { OFF, @@ -334,11 +410,50 @@ impl HandState { } } +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::THUMB_METACARPAL => (0.01, Color::RED), + HandBone::THUMB_PROXIMAL => (0.008, Color::RED), + HandBone::THUMB_DISTAL => (0.006, Color::RED), + HandBone::THUMB_TIP => (0.004, Color::RED), + HandBone::INDEX_METACARPAL => (0.01, Color::ORANGE), + HandBone::INDEX_PROXIMAL => (0.008, Color::ORANGE), + HandBone::INDEX_INTERMEDIATE => (0.006, Color::ORANGE), + HandBone::INDEX_DISTAL => (0.004, Color::ORANGE), + HandBone::INDEX_TIP => (0.002, Color::ORANGE), + HandBone::MIDDLE_METACARPAL => (0.01, Color::YELLOW), + HandBone::MIDDLE_PROXIMAL => (0.008, Color::YELLOW), + HandBone::MIDDLE_INTERMEDIATE => (0.006, Color::YELLOW), + HandBone::MIDDLE_DISTAL => (0.004, Color::YELLOW), + HandBone::MIDDLE_TIP => (0.002, Color::YELLOW), + HandBone::RING_METACARPAL => (0.01, Color::GREEN), + HandBone::RING_PROXIMAL => (0.008, Color::GREEN), + HandBone::RING_INTERMEDIATE => (0.006, Color::GREEN), + HandBone::RING_DISTAL => (0.004, Color::GREEN), + HandBone::RING_TIP => (0.002, Color::GREEN), + HandBone::LITTLE_METACARPAL => (0.01, Color::BLUE), + HandBone::LITTLE_PROXIMAL => (0.008, Color::BLUE), + HandBone::LITTLE_INTERMEDIATE => (0.006, Color::BLUE), + HandBone::LITTLE_DISTAL => (0.004, Color::BLUE), + HandBone::LITTLE_TIP => (0.002, Color::BLUE), + } +} + fn draw_hand_bones( mut gizmos: &mut Gizmos, 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 { @@ -355,6 +470,9 @@ fn draw_hand_bones( 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(); @@ -384,6 +502,10 @@ fn draw_hand_bones( //draw controller-palm bone(should be zero length) let palm = hand_transform_array[HandJoint::PALM]; gizmos.ray(hand_translation, palm.translation, Color::WHITE); + calc_transforms[HandJoint::PALM] = Transform { + translation: hand_translation + palm.translation, + ..default() + }; //draw palm-wrist let wrist = hand_transform_array[HandJoint::WRIST]; gizmos.ray( @@ -391,6 +513,10 @@ fn draw_hand_bones( palm_quat.mul_vec3(wrist.translation), Color::GRAY, ); + calc_transforms[HandJoint::WRIST] = Transform { + translation: hand_translation + palm.translation + palm_quat.mul_vec3(wrist.translation), + ..default() + }; //thumb //better finger drawing? @@ -420,6 +546,11 @@ fn draw_hand_bones( 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]; @@ -431,6 +562,11 @@ fn draw_hand_bones( 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() + }; } } } @@ -463,6 +599,11 @@ fn draw_hand_bones( 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]; @@ -474,6 +615,11 @@ fn draw_hand_bones( 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() + }; } } } @@ -505,6 +651,11 @@ fn draw_hand_bones( 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]; @@ -516,6 +667,11 @@ fn draw_hand_bones( 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() + }; } } } @@ -546,6 +702,11 @@ fn draw_hand_bones( 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]; @@ -557,6 +718,11 @@ fn draw_hand_bones( 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() + }; } } } @@ -588,6 +754,11 @@ fn draw_hand_bones( 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]; @@ -599,9 +770,54 @@ fn draw_hand_bones( 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::THUMB_METACARPAL => HandJoint::THUMB_METACARPAL, + HandBone::THUMB_PROXIMAL => HandJoint::THUMB_PROXIMAL, + HandBone::THUMB_DISTAL => HandJoint::THUMB_DISTAL, + HandBone::THUMB_TIP => HandJoint::THUMB_TIP, + HandBone::INDEX_METACARPAL => HandJoint::INDEX_METACARPAL, + HandBone::INDEX_PROXIMAL => HandJoint::INDEX_PROXIMAL, + HandBone::INDEX_INTERMEDIATE => HandJoint::INDEX_INTERMEDIATE, + HandBone::INDEX_DISTAL => HandJoint::INDEX_DISTAL, + HandBone::INDEX_TIP => HandJoint::INDEX_TIP, + HandBone::MIDDLE_METACARPAL => HandJoint::MIDDLE_METACARPAL, + HandBone::MIDDLE_PROXIMAL => HandJoint::MIDDLE_PROXIMAL, + HandBone::MIDDLE_INTERMEDIATE => HandJoint::MIDDLE_INTERMEDIATE, + HandBone::MIDDLE_DISTAL => HandJoint::MIDDLE_DISTAL, + HandBone::MIDDLE_TIP => HandJoint::MIDDLE_TIP, + HandBone::RING_METACARPAL => HandJoint::RING_METACARPAL, + HandBone::RING_PROXIMAL => HandJoint::RING_PROXIMAL, + HandBone::RING_INTERMEDIATE => HandJoint::RING_INTERMEDIATE, + HandBone::RING_DISTAL => HandJoint::RING_DISTAL, + HandBone::RING_TIP => HandJoint::RING_TIP, + HandBone::LITTLE_METACARPAL => HandJoint::LITTLE_METACARPAL, + HandBone::LITTLE_PROXIMAL => HandJoint::LITTLE_PROXIMAL, + HandBone::LITTLE_INTERMEDIATE => HandJoint::LITTLE_INTERMEDIATE, + HandBone::LITTLE_DISTAL => HandJoint::LITTLE_DISTAL, + HandBone::LITTLE_TIP => HandJoint::LITTLE_TIP, + } } fn get_bone_curl_angle(bone: HandJoint, curl: f32) -> f32 { @@ -617,320 +833,6 @@ fn get_bone_curl_angle(bone: HandJoint, curl: f32) -> f32 { return curl_angle; } -fn draw_hand(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: Hand) { - //draw debug for controller grip center to match palm to - let hand_translation = controller_transform.translation; - let hand_quat = controller_transform.rotation; - //we need to flip this i dont know why - let flip = Quat::from_rotation_x(PI); - let controller_backward = hand_quat.mul_quat(flip); - - let test_hand_pose = get_test_hand_pose_array(); - - let hand_pose = flip_hand_pose(test_hand_pose.clone(), hand); - - // let hand_transform_array: [Transform; 26] = pose_array_to_transform_array(hand_pose); - let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); - - let palm = hand_transform_array[HandJoint::PALM]; - gizmos.sphere( - palm.translation + hand_translation, - palm.rotation.mul_quat(controller_backward), - 0.01, - Color::WHITE, - ); - - let wrist = hand_transform_array[HandJoint::WRIST]; - draw_joint( - &mut gizmos, - wrist.translation, - wrist.rotation, - 0.01, - Color::GRAY, - controller_backward, - hand_translation, - ); - - let thumb_meta = hand_transform_array[HandJoint::THUMB_METACARPAL]; - draw_joint( - &mut gizmos, - thumb_meta.translation, - thumb_meta.rotation, - 0.01, - Color::RED, - controller_backward, - hand_translation, - ); - - let thumb_prox = hand_transform_array[HandJoint::THUMB_PROXIMAL]; - draw_joint( - &mut gizmos, - thumb_prox.translation, - thumb_prox.rotation, - 0.008, - Color::RED, - controller_backward, - hand_translation, - ); - let thumb_dist = hand_transform_array[HandJoint::THUMB_DISTAL]; - draw_joint( - &mut gizmos, - thumb_dist.translation, - thumb_dist.rotation, - 0.006, - Color::RED, - controller_backward, - hand_translation, - ); - let thumb_tip = hand_transform_array[HandJoint::THUMB_TIP]; - draw_joint( - &mut gizmos, - thumb_tip.translation, - thumb_tip.rotation, - 0.004, - Color::RED, - controller_backward, - hand_translation, - ); - - let index_meta = hand_transform_array[HandJoint::INDEX_METACARPAL]; - draw_joint( - &mut gizmos, - index_meta.translation, - index_meta.rotation, - 0.01, - Color::ORANGE, - controller_backward, - hand_translation, - ); - let index_prox = hand_transform_array[HandJoint::INDEX_PROXIMAL]; - draw_joint( - &mut gizmos, - index_prox.translation, - index_prox.rotation, - 0.008, - Color::ORANGE, - controller_backward, - hand_translation, - ); - - let index_inter = hand_transform_array[HandJoint::INDEX_INTERMEDIATE]; - draw_joint( - &mut gizmos, - index_inter.translation, - index_inter.rotation, - 0.006, - Color::ORANGE, - controller_backward, - hand_translation, - ); - - let index_dist = hand_transform_array[HandJoint::INDEX_DISTAL]; - draw_joint( - &mut gizmos, - index_dist.translation, - index_dist.rotation, - 0.004, - Color::ORANGE, - controller_backward, - hand_translation, - ); - - let index_tip = hand_transform_array[HandJoint::INDEX_TIP]; - draw_joint( - &mut gizmos, - index_tip.translation, - index_tip.rotation, - 0.002, - Color::ORANGE, - controller_backward, - hand_translation, - ); - - let middle_meta = hand_transform_array[HandJoint::MIDDLE_METACARPAL]; - draw_joint( - &mut gizmos, - middle_meta.translation, - middle_meta.rotation, - 0.01, - Color::YELLOW, - controller_backward, - hand_translation, - ); - let middle_prox = hand_transform_array[HandJoint::MIDDLE_PROXIMAL]; - draw_joint( - &mut gizmos, - middle_prox.translation, - middle_prox.rotation, - 0.008, - Color::YELLOW, - controller_backward, - hand_translation, - ); - let middle_inter = hand_transform_array[HandJoint::MIDDLE_INTERMEDIATE]; - draw_joint( - &mut gizmos, - middle_inter.translation, - middle_inter.rotation, - 0.006, - Color::YELLOW, - controller_backward, - hand_translation, - ); - let middle_dist = hand_transform_array[HandJoint::MIDDLE_DISTAL]; - draw_joint( - &mut gizmos, - middle_dist.translation, - middle_dist.rotation, - 0.004, - Color::YELLOW, - controller_backward, - hand_translation, - ); - let middle_tip = hand_transform_array[HandJoint::MIDDLE_TIP]; - draw_joint( - &mut gizmos, - middle_tip.translation, - middle_tip.rotation, - 0.002, - Color::YELLOW, - controller_backward, - hand_translation, - ); - - let ring_meta = hand_transform_array[HandJoint::RING_METACARPAL]; - draw_joint( - &mut gizmos, - ring_meta.translation, - ring_meta.rotation, - 0.01, - Color::GREEN, - controller_backward, - hand_translation, - ); - let ring_prox = hand_transform_array[HandJoint::RING_PROXIMAL]; - draw_joint( - &mut gizmos, - ring_prox.translation, - ring_prox.rotation, - 0.008, - Color::GREEN, - controller_backward, - hand_translation, - ); - let ring_inter = hand_transform_array[HandJoint::RING_INTERMEDIATE]; - draw_joint( - &mut gizmos, - ring_inter.translation, - ring_inter.rotation, - 0.006, - Color::GREEN, - controller_backward, - hand_translation, - ); - let ring_dist = hand_transform_array[HandJoint::RING_DISTAL]; - draw_joint( - &mut gizmos, - ring_dist.translation, - ring_dist.rotation, - 0.004, - Color::GREEN, - controller_backward, - hand_translation, - ); - let ring_tip = hand_transform_array[HandJoint::RING_TIP]; - draw_joint( - &mut gizmos, - ring_tip.translation, - ring_tip.rotation, - 0.002, - Color::GREEN, - controller_backward, - hand_translation, - ); - - let little_meta = hand_transform_array[HandJoint::LITTLE_METACARPAL]; - draw_joint( - &mut gizmos, - little_meta.translation, - little_meta.rotation, - 0.01, - Color::BLUE, - controller_backward, - hand_translation, - ); - let little_prox = hand_transform_array[HandJoint::LITTLE_PROXIMAL]; - draw_joint( - &mut gizmos, - little_prox.translation, - little_prox.rotation, - 0.008, - Color::BLUE, - controller_backward, - hand_translation, - ); - let little_inter = hand_transform_array[HandJoint::LITTLE_INTERMEDIATE]; - draw_joint( - &mut gizmos, - little_inter.translation, - little_inter.rotation, - 0.006, - Color::BLUE, - controller_backward, - hand_translation, - ); - let little_dist = hand_transform_array[HandJoint::LITTLE_DISTAL]; - draw_joint( - &mut gizmos, - little_dist.translation, - little_dist.rotation, - 0.004, - Color::BLUE, - controller_backward, - hand_translation, - ); - let little_tip = hand_transform_array[HandJoint::LITTLE_TIP]; - draw_joint( - &mut gizmos, - little_tip.translation, - little_tip.rotation, - 0.002, - Color::BLUE, - controller_backward, - hand_translation, - ); -} - -fn flip_hand_pose(hand_pose: [Posef; 26], hand: Hand) -> [Posef; 26] { - let mut new_pose = hand_pose; - match hand { - Hand::Left => { - for pose in new_pose.iter_mut() { - pose.position.x = -pose.position.x; - } - } - Hand::Right => (), - } - return new_pose; -} - -fn draw_joint( - gizmos: &mut Gizmos, - joint_pos: Vec3, - joint_rot: Quat, - radius: f32, - color: Color, - controller_backwards: Quat, - offset: Vec3, -) { - gizmos.sphere( - controller_backwards.mul_vec3(joint_pos) + offset, - joint_rot, - radius, - color, - ); -} - fn log_hand(hand_pose: [Posef; 26]) { let palm_wrist = hand_pose[HandJoint::WRIST].position.to_vec3() - hand_pose[HandJoint::PALM].position.to_vec3(); diff --git a/src/xr_input/mod.rs b/src/xr_input/mod.rs index 1ee3e71..a81badc 100644 --- a/src/xr_input/mod.rs +++ b/src/xr_input/mod.rs @@ -13,7 +13,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}; @@ -28,7 +28,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, From cae43d417569658455fbb35205f7bc585bb78b2c Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 00:16:14 -0400 Subject: [PATCH 19/66] slight cleaning --- examples/xr.rs | 66 +++++++++----------------------------------------- 1 file changed, 11 insertions(+), 55 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 23de2bd..91f6587 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -30,7 +30,7 @@ fn main() { .add_systems(Startup, setup) .add_systems(Update, proto_locomotion) .add_systems(Startup, spawn_controllers_example) - .add_systems(Update, draw_skeleton_hands) + .add_systems(Update, update_emulated_hand_skeletons) .add_systems(PreUpdate, update_hand_states) .add_systems(PostUpdate, draw_hand_entities) .add_systems(Startup, spawn_hand_entities) @@ -245,8 +245,7 @@ pub fn update_hand_states( } } -fn draw_skeleton_hands( - mut gizmos: Gizmos, +fn update_emulated_hand_skeletons( right_controller_query: Query<(&GlobalTransform, With)>, left_controller_query: Query<(&GlobalTransform, With)>, hand_states_option: Option>, @@ -259,8 +258,7 @@ fn draw_skeleton_hands( .unwrap() .0 .compute_transform(); - draw_hand_bones( - &mut gizmos, + update_hand_bones_emulated( left_hand_transform, Hand::Left, hands.left, @@ -271,9 +269,7 @@ fn draw_skeleton_hands( .unwrap() .0 .compute_transform(); - // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); - draw_hand_bones( - &mut gizmos, + update_hand_bones_emulated( right_hand_transform, Hand::Right, hands.right, @@ -448,8 +444,7 @@ fn get_bone_gizmo_style(hand_bone: &HandBone) -> (f32, Color) { } } -fn draw_hand_bones( - mut gizmos: &mut Gizmos, +fn update_hand_bones_emulated( controller_transform: Transform, hand: Hand, hand_state: HandState, @@ -477,49 +472,26 @@ fn draw_hand_bones( let grip_curl = hand_state.grip; let index_curl = hand_state.get_index_curl(); let thumb_curl = hand_state.get_thumb_curl(); - //get paml quat + //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); - //draw debug rays - // gizmos.ray( - // hand_translation, - // palm_quat.mul_vec3(Vec3::Z * 0.2), - // Color::BLUE, - // ); - // gizmos.ray( - // hand_translation, - // palm_quat.mul_vec3(Vec3::Y * 0.2), - // Color::GREEN, - // ); - // gizmos.ray( - // hand_translation, - // palm_quat.mul_vec3(Vec3::X * 0.2), - // Color::RED, - // ); //get simulated bones let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); - //draw controller-palm bone(should be zero length) + //palm let palm = hand_transform_array[HandJoint::PALM]; - gizmos.ray(hand_translation, palm.translation, Color::WHITE); calc_transforms[HandJoint::PALM] = Transform { translation: hand_translation + palm.translation, ..default() }; - //draw palm-wrist + //wrist let wrist = hand_transform_array[HandJoint::WRIST]; - gizmos.ray( - hand_translation + palm.translation, - palm_quat.mul_vec3(wrist.translation), - Color::GRAY, - ); calc_transforms[HandJoint::WRIST] = Transform { translation: hand_translation + palm.translation + palm_quat.mul_vec3(wrist.translation), ..default() }; //thumb - //better finger drawing? let thumb_joints = [ HandJoint::THUMB_METACARPAL, HandJoint::THUMB_PROXIMAL, @@ -529,7 +501,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::RED; 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); @@ -542,7 +513,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -558,7 +528,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); @@ -572,7 +541,6 @@ fn draw_hand_bones( } //index - //better finger drawing? let thumb_joints = [ HandJoint::INDEX_METACARPAL, HandJoint::INDEX_PROXIMAL, @@ -583,7 +551,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::ORANGE; 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() { @@ -595,7 +562,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -611,7 +577,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); @@ -624,7 +589,7 @@ fn draw_hand_bones( } } - //better finger drawing? + //middle let thumb_joints = [ HandJoint::MIDDLE_METACARPAL, HandJoint::MIDDLE_PROXIMAL, @@ -635,7 +600,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::YELLOW; 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() { @@ -647,7 +611,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -663,7 +626,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); @@ -675,7 +637,7 @@ fn draw_hand_bones( } } } - //better finger drawing? + //ring let thumb_joints = [ HandJoint::RING_METACARPAL, HandJoint::RING_PROXIMAL, @@ -686,7 +648,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::GREEN; 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() { @@ -698,7 +659,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -714,7 +674,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); @@ -727,7 +686,7 @@ fn draw_hand_bones( } } - //better finger drawing? + //little let thumb_joints = [ HandJoint::LITTLE_METACARPAL, HandJoint::LITTLE_PROXIMAL, @@ -738,7 +697,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::BLUE; 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() { @@ -750,7 +708,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -766,7 +723,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); From 5c507413c840f22b60a9439f2a20fca9cb077630 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 00:17:31 -0400 Subject: [PATCH 20/66] cargo fix --- examples/xr.rs | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 91f6587..ad738a9 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,22 +1,22 @@ use std::f32::consts::PI; -use std::ops::Mul; + use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; -use bevy::math::vec3; + use bevy::transform::components::Transform; -use bevy::{gizmos, prelude::*}; +use bevy::{prelude::*}; use bevy_openxr::input::XrInput; use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; -use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; + use bevy_openxr::xr_input::hand_poses::*; 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, }; -use bevy_openxr::xr_input::{Hand, QuatConv, Vec3Conv}; +use bevy_openxr::xr_input::{Hand, Vec3Conv}; use bevy_openxr::DefaultXrPlugins; -use openxr::{HandJoint, Posef, Quaternionf, Vector3f}; +use openxr::{HandJoint, Posef}; fn main() { color_eyre::install().unwrap(); @@ -790,7 +790,7 @@ fn get_bone_curl_angle(bone: HandJoint, curl: f32) -> f32 { } fn log_hand(hand_pose: [Posef; 26]) { - let palm_wrist = hand_pose[HandJoint::WRIST].position.to_vec3() + let _palm_wrist = hand_pose[HandJoint::WRIST].position.to_vec3() - hand_pose[HandJoint::PALM].position.to_vec3(); info!( "palm-wrist: {}", From 539753790a385e978698ec21dcffd72d56ef8ffa Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 00:19:43 -0400 Subject: [PATCH 21/66] mass rename --- examples/xr.rs | 210 ++++++++++++++++++++++++------------------------- 1 file changed, 105 insertions(+), 105 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index ad738a9..d47b6f8 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -4,7 +4,7 @@ use std::f32::consts::PI; use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::transform::components::Transform; -use bevy::{prelude::*}; +use bevy::prelude::*; use bevy_openxr::input::XrInput; use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; @@ -92,32 +92,32 @@ fn setup( pub fn spawn_hand_entities(mut commands: Commands) { let hands = [Hand::Left, Hand::Right]; let bones = [ - HandBone::PALM, - HandBone::WRIST, - HandBone::THUMB_METACARPAL, - HandBone::THUMB_PROXIMAL, - HandBone::THUMB_DISTAL, - HandBone::THUMB_TIP, - HandBone::INDEX_METACARPAL, - HandBone::INDEX_PROXIMAL, - HandBone::INDEX_INTERMEDIATE, - HandBone::INDEX_DISTAL, - HandBone::INDEX_TIP, - HandBone::MIDDLE_METACARPAL, - HandBone::MIDDLE_PROXIMAL, - HandBone::MIDDLE_INTERMEDIATE, - HandBone::MIDDLE_DISTAL, - HandBone::MIDDLE_TIP, - HandBone::RING_METACARPAL, - HandBone::RING_PROXIMAL, - HandBone::RING_INTERMEDIATE, - HandBone::RING_DISTAL, - HandBone::RING_TIP, - HandBone::LITTLE_METACARPAL, - HandBone::LITTLE_PROXIMAL, - HandBone::LITTLE_INTERMEDIATE, - HandBone::LITTLE_DISTAL, - HandBone::LITTLE_TIP, + 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, ]; for hand in hands.iter() { @@ -282,32 +282,32 @@ fn update_emulated_hand_skeletons( #[derive(Component, Debug, Clone, Copy)] pub enum HandBone { - PALM, - WRIST, - THUMB_METACARPAL, - THUMB_PROXIMAL, - THUMB_DISTAL, - THUMB_TIP, - INDEX_METACARPAL, - INDEX_PROXIMAL, - INDEX_INTERMEDIATE, - INDEX_DISTAL, - INDEX_TIP, - MIDDLE_METACARPAL, - MIDDLE_PROXIMAL, - MIDDLE_INTERMEDIATE, - MIDDLE_DISTAL, - MIDDLE_TIP, - RING_METACARPAL, - RING_PROXIMAL, - RING_INTERMEDIATE, - RING_DISTAL, - RING_TIP, - LITTLE_METACARPAL, - LITTLE_PROXIMAL, - LITTLE_INTERMEDIATE, - LITTLE_DISTAL, - LITTLE_TIP, + 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, } #[derive(Clone, Copy)] @@ -415,32 +415,32 @@ fn draw_hand_entities(mut gizmos: Gizmos, query: Query<(&Transform, &HandBone)>) 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::THUMB_METACARPAL => (0.01, Color::RED), - HandBone::THUMB_PROXIMAL => (0.008, Color::RED), - HandBone::THUMB_DISTAL => (0.006, Color::RED), - HandBone::THUMB_TIP => (0.004, Color::RED), - HandBone::INDEX_METACARPAL => (0.01, Color::ORANGE), - HandBone::INDEX_PROXIMAL => (0.008, Color::ORANGE), - HandBone::INDEX_INTERMEDIATE => (0.006, Color::ORANGE), - HandBone::INDEX_DISTAL => (0.004, Color::ORANGE), - HandBone::INDEX_TIP => (0.002, Color::ORANGE), - HandBone::MIDDLE_METACARPAL => (0.01, Color::YELLOW), - HandBone::MIDDLE_PROXIMAL => (0.008, Color::YELLOW), - HandBone::MIDDLE_INTERMEDIATE => (0.006, Color::YELLOW), - HandBone::MIDDLE_DISTAL => (0.004, Color::YELLOW), - HandBone::MIDDLE_TIP => (0.002, Color::YELLOW), - HandBone::RING_METACARPAL => (0.01, Color::GREEN), - HandBone::RING_PROXIMAL => (0.008, Color::GREEN), - HandBone::RING_INTERMEDIATE => (0.006, Color::GREEN), - HandBone::RING_DISTAL => (0.004, Color::GREEN), - HandBone::RING_TIP => (0.002, Color::GREEN), - HandBone::LITTLE_METACARPAL => (0.01, Color::BLUE), - HandBone::LITTLE_PROXIMAL => (0.008, Color::BLUE), - HandBone::LITTLE_INTERMEDIATE => (0.006, Color::BLUE), - HandBone::LITTLE_DISTAL => (0.004, Color::BLUE), - HandBone::LITTLE_TIP => (0.002, Color::BLUE), + 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), } } @@ -747,32 +747,32 @@ fn update_hand_bones_emulated( fn match_index(handbone: &HandBone) -> HandJoint { match handbone { - HandBone::PALM => HandJoint::PALM, - HandBone::WRIST => HandJoint::WRIST, - HandBone::THUMB_METACARPAL => HandJoint::THUMB_METACARPAL, - HandBone::THUMB_PROXIMAL => HandJoint::THUMB_PROXIMAL, - HandBone::THUMB_DISTAL => HandJoint::THUMB_DISTAL, - HandBone::THUMB_TIP => HandJoint::THUMB_TIP, - HandBone::INDEX_METACARPAL => HandJoint::INDEX_METACARPAL, - HandBone::INDEX_PROXIMAL => HandJoint::INDEX_PROXIMAL, - HandBone::INDEX_INTERMEDIATE => HandJoint::INDEX_INTERMEDIATE, - HandBone::INDEX_DISTAL => HandJoint::INDEX_DISTAL, - HandBone::INDEX_TIP => HandJoint::INDEX_TIP, - HandBone::MIDDLE_METACARPAL => HandJoint::MIDDLE_METACARPAL, - HandBone::MIDDLE_PROXIMAL => HandJoint::MIDDLE_PROXIMAL, - HandBone::MIDDLE_INTERMEDIATE => HandJoint::MIDDLE_INTERMEDIATE, - HandBone::MIDDLE_DISTAL => HandJoint::MIDDLE_DISTAL, - HandBone::MIDDLE_TIP => HandJoint::MIDDLE_TIP, - HandBone::RING_METACARPAL => HandJoint::RING_METACARPAL, - HandBone::RING_PROXIMAL => HandJoint::RING_PROXIMAL, - HandBone::RING_INTERMEDIATE => HandJoint::RING_INTERMEDIATE, - HandBone::RING_DISTAL => HandJoint::RING_DISTAL, - HandBone::RING_TIP => HandJoint::RING_TIP, - HandBone::LITTLE_METACARPAL => HandJoint::LITTLE_METACARPAL, - HandBone::LITTLE_PROXIMAL => HandJoint::LITTLE_PROXIMAL, - HandBone::LITTLE_INTERMEDIATE => HandJoint::LITTLE_INTERMEDIATE, - HandBone::LITTLE_DISTAL => HandJoint::LITTLE_DISTAL, - HandBone::LITTLE_TIP => HandJoint::LITTLE_TIP, + 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, } } From 2f06a01f15f86a9647c93056ab32136dae8b5a81 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 21:04:20 -0400 Subject: [PATCH 22/66] create resource to help with entity selection --- examples/xr.rs | 80 +++++++++++++++++++++++++++++++++++----- src/xr_input/hand.rs | 88 ++++++++++++++++++++++++++++++++++++++++++-- src/xr_input/mod.rs | 1 + 3 files changed, 157 insertions(+), 12 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index d47b6f8..4fb94bb 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,13 +1,13 @@ use std::f32::consts::PI; - use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; -use bevy::transform::components::Transform; use bevy::prelude::*; +use bevy::transform::components::Transform; use bevy_openxr::input::XrInput; use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; +use bevy_openxr::xr_input::hand::HandsResource; use bevy_openxr::xr_input::hand_poses::*; use bevy_openxr::xr_input::oculus_touch::OculusController; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; @@ -119,17 +119,79 @@ pub fn spawn_hand_entities(mut commands: Commands) { HandBone::LittleDistal, HandBone::LittleTip, ]; - + //hand resource + let mut hand_resource = HandsResource { ..default() }; for hand in hands.iter() { for bone in bones.iter() { - commands.spawn(( - SpatialBundle::default(), - bone.clone(), - OpenXRTracker, - hand.clone(), - )); + 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); } pub fn update_hand_states( diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs index 0fcd267..b84e0dc 100644 --- a/src/xr_input/hand.rs +++ b/src/xr_input/hand.rs @@ -1,3 +1,85 @@ -pub struct Hand { - pub HandJointPoses: [Posef; 26] -} \ No newline at end of file +use bevy::prelude::{Entity, Resource}; + +#[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 } + } +} diff --git a/src/xr_input/mod.rs b/src/xr_input/mod.rs index a81badc..65eef19 100644 --- a/src/xr_input/mod.rs +++ b/src/xr_input/mod.rs @@ -5,6 +5,7 @@ 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; From 398c65f1fd0f3f1f89220059768d913fee114e9b Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 10 Oct 2023 13:05:09 -0400 Subject: [PATCH 23/66] moved everything to the hand file --- examples/xr.rs | 909 +------------------------------------------ src/xr_input/hand.rs | 907 +++++++++++++++++++++++++++++++++++++++++- 2 files changed, 909 insertions(+), 907 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 4fb94bb..cbc7ac7 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,22 +1,16 @@ -use std::f32::consts::PI; + use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; -use bevy_openxr::input::XrInput; -use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; -use bevy_openxr::xr_input::hand::HandsResource; -use bevy_openxr::xr_input::hand_poses::*; -use bevy_openxr::xr_input::oculus_touch::OculusController; +use bevy_openxr::xr_input::hand::{spawn_hand_entities, update_hand_states, HandStatesResource, update_emulated_hand_skeletons, draw_hand_entities}; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, }; -use bevy_openxr::xr_input::{Hand, Vec3Conv}; use bevy_openxr::DefaultXrPlugins; -use openxr::{HandJoint, Posef}; fn main() { color_eyre::install().unwrap(); @@ -89,903 +83,6 @@ fn setup( },)); } -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); -} - -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"), - } -} - -fn update_emulated_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)>, -) { - 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"), - } -} - -#[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, -} - -#[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 { - grip: f32, - trigger_state: TriggerState, - a_button: ButtonState, - b_button: ButtonState, - thumbstick: ThumbstickState, -} - -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(), - } - } -} - -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; - } -} - -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), - } -} - -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() - ); -} - fn spawn_controllers_example(mut commands: Commands) { //left hand commands.spawn(( @@ -1001,4 +98,4 @@ fn spawn_controllers_example(mut commands: Commands) { OpenXRTracker, SpatialBundle::default(), )); -} +} \ No newline at end of file diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs index b84e0dc..f02b5c4 100644 --- a/src/xr_input/hand.rs +++ b/src/xr_input/hand.rs @@ -1,4 +1,11 @@ -use bevy::prelude::{Entity, Resource}; +use std::f32::consts::PI; + +use bevy::prelude::{Entity, Resource, Component, default, SpatialBundle, Commands, Res, ResMut, info, Transform, Query, Quat, Vec3, GlobalTransform, With, Gizmos, Color}; +use openxr::{HandJoint, Posef}; + +use crate::{resources::{XrFrameState, XrInstance, XrSession}, input::XrInput, xr_input::Vec3Conv}; + +use super::{trackers::{OpenXRTracker, OpenXRRightController, OpenXRLeftController}, Hand, oculus_touch::OculusController, hand_poses::get_simulated_open_hand_transforms}; #[derive(Resource, Default)] pub struct HandsResource { @@ -83,3 +90,901 @@ impl Default for LittleResource { 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_emulated_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)>, +) { + 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"), + } +} + +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), + } +} \ No newline at end of file From 508a2acbdd63d883aa7feacdc4f1e94065686083 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sat, 14 Oct 2023 11:02:49 -0400 Subject: [PATCH 24/66] look upon my bad code --- examples/xr.rs | 10 +-- src/xr_input/hand.rs | 174 +++++++++++++++++++++++++++++++++---------- 2 files changed, 138 insertions(+), 46 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index cbc7ac7..363cbe9 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -5,7 +5,7 @@ use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; -use bevy_openxr::xr_input::hand::{spawn_hand_entities, update_hand_states, HandStatesResource, update_emulated_hand_skeletons, draw_hand_entities}; +use bevy_openxr::xr_input::hand::OpenXrHandInput; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, @@ -23,13 +23,9 @@ fn main() { .add_plugins(FrameTimeDiagnosticsPlugin) .add_systems(Startup, setup) .add_systems(Update, proto_locomotion) - .add_systems(Startup, spawn_controllers_example) - .add_systems(Update, update_emulated_hand_skeletons) - .add_systems(PreUpdate, update_hand_states) - .add_systems(PostUpdate, draw_hand_entities) - .add_systems(Startup, spawn_hand_entities) .insert_resource(PrototypeLocomotionConfig::default()) - .insert_resource(HandStatesResource::default()) + .add_systems(Startup, spawn_controllers_example) + .add_plugins(OpenXrHandInput) .run(); } diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs index f02b5c4..63e5ef8 100644 --- a/src/xr_input/hand.rs +++ b/src/xr_input/hand.rs @@ -1,11 +1,51 @@ use std::f32::consts::PI; -use bevy::prelude::{Entity, Resource, Component, default, SpatialBundle, Commands, Res, ResMut, info, Transform, Query, Quat, Vec3, GlobalTransform, With, Gizmos, Color}; +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::{resources::{XrFrameState, XrInstance, XrSession}, input::XrInput, xr_input::Vec3Conv}; +use crate::{ + input::XrInput, + resources::{XrFrameState, XrInstance, XrSession}, + xr_input::Vec3Conv, +}; -use super::{trackers::{OpenXRTracker, OpenXRRightController, OpenXRLeftController}, Hand, oculus_touch::OculusController, hand_poses::get_simulated_open_hand_transforms}; +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(PostUpdate, draw_hand_entities) + .add_systems(Startup, spawn_hand_entities) + .insert_resource(HandStatesResource::default()) + .insert_resource(HandInputSource::default()); + } +} + +#[derive(Resource)] +pub enum HandInputSource { + Emulated, + OpenXr, +} + +impl Default for HandInputSource { + fn default() -> Self { + HandInputSource::Emulated + } +} #[derive(Resource, Default)] pub struct HandsResource { @@ -25,7 +65,15 @@ pub struct HandResource { 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() } + Self { + palm: Entity::PLACEHOLDER, + wrist: Entity::PLACEHOLDER, + thumb: Default::default(), + index: Default::default(), + middle: Default::default(), + ring: Default::default(), + little: Default::default(), + } } } @@ -38,7 +86,12 @@ pub struct ThumbResource { impl Default for ThumbResource { fn default() -> Self { - Self { metacarpal: Entity::PLACEHOLDER, proximal: Entity::PLACEHOLDER, distal: Entity::PLACEHOLDER, tip: Entity::PLACEHOLDER } + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } } } pub struct IndexResource { @@ -51,7 +104,13 @@ pub struct IndexResource { impl Default for IndexResource { fn default() -> Self { - Self { metacarpal: Entity::PLACEHOLDER, proximal: Entity::PLACEHOLDER, intermediate: Entity::PLACEHOLDER, distal: Entity::PLACEHOLDER, tip: Entity::PLACEHOLDER } + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + intermediate: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } } } pub struct MiddleResource { @@ -63,7 +122,13 @@ pub struct MiddleResource { } impl Default for MiddleResource { fn default() -> Self { - Self { metacarpal: Entity::PLACEHOLDER, proximal: Entity::PLACEHOLDER, intermediate: Entity::PLACEHOLDER, distal: Entity::PLACEHOLDER, tip: Entity::PLACEHOLDER } + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + intermediate: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } } } pub struct RingResource { @@ -75,7 +140,13 @@ pub struct RingResource { } impl Default for RingResource { fn default() -> Self { - Self { metacarpal: Entity::PLACEHOLDER, proximal: Entity::PLACEHOLDER, intermediate: Entity::PLACEHOLDER, distal: Entity::PLACEHOLDER, tip: Entity::PLACEHOLDER } + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + intermediate: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } } } pub struct LittleResource { @@ -87,7 +158,13 @@ pub struct LittleResource { } impl Default for LittleResource { fn default() -> Self { - Self { metacarpal: Entity::PLACEHOLDER, proximal: Entity::PLACEHOLDER, intermediate: Entity::PLACEHOLDER, distal: Entity::PLACEHOLDER, tip: Entity::PLACEHOLDER } + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + intermediate: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } } } @@ -176,7 +253,9 @@ pub fn spawn_hand_entities(mut commands: Commands) { 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::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, @@ -186,7 +265,9 @@ pub fn spawn_hand_entities(mut commands: Commands) { 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::LittleIntermediate => { + hand_resource.right.little.intermediate = boneid + } HandBone::LittleDistal => hand_resource.right.little.distal = boneid, HandBone::LittleTip => hand_resource.right.little.tip = boneid, }, @@ -915,39 +996,54 @@ fn log_hand(hand_pose: [Posef; 26]) { ); } - -pub fn update_emulated_hand_skeletons( +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 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, - ); + 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; } - None => info!("hand states resource not initialized yet"), } } @@ -987,4 +1083,4 @@ fn get_bone_gizmo_style(hand_bone: &HandBone) -> (f32, Color) { HandBone::LittleDistal => (0.004, Color::BLUE), HandBone::LittleTip => (0.002, Color::BLUE), } -} \ No newline at end of file +} From 6770c975d76f482121caa04991f51b41b5f8b0f9 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sat, 14 Oct 2023 11:05:06 -0400 Subject: [PATCH 25/66] pulled debug rendering into its own plugin --- examples/xr.rs | 3 ++- src/xr_input/hand.rs | 11 ++++++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 363cbe9..f0d468d 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -5,7 +5,7 @@ use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; -use bevy_openxr::xr_input::hand::OpenXrHandInput; +use bevy_openxr::xr_input::hand::{OpenXrHandInput, HandInputDebugRenderer}; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, @@ -26,6 +26,7 @@ fn main() { .insert_resource(PrototypeLocomotionConfig::default()) .add_systems(Startup, spawn_controllers_example) .add_plugins(OpenXrHandInput) + .add_plugins(HandInputDebugRenderer) .run(); } diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs index 63e5ef8..f8a70ac 100644 --- a/src/xr_input/hand.rs +++ b/src/xr_input/hand.rs @@ -28,13 +28,22 @@ 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(PostUpdate, draw_hand_entities) .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, From bd89642e029d9b9ebfc88394ebf440539a70b3d5 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 26 Sep 2023 12:54:52 -0400 Subject: [PATCH 26/66] got a hand array --- examples/xr.rs | 105 ++++++++++++++++++++++++++++++++++++++++++- src/xr_input/hand.rs | 3 ++ 2 files changed, 107 insertions(+), 1 deletion(-) create mode 100644 src/xr_input/hand.rs diff --git a/examples/xr.rs b/examples/xr.rs index 548fe15..80eb3b7 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,12 +1,14 @@ use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; +use bevy_openxr::xr_input::{Vec3Conv, QuatConv}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, }; use bevy_openxr::DefaultXrPlugins; +use openxr::{Posef, Quaternionf, Vector3f, HandJoint}; fn main() { color_eyre::install().unwrap(); @@ -20,6 +22,7 @@ fn main() { .add_systems(Startup, setup) .add_systems(Update, proto_locomotion) .add_systems(Startup, spawn_controllers_example) + .add_systems(Update, spawn_skeleton) .insert_resource(PrototypeLocomotionConfig::default()) .run(); } @@ -62,11 +65,111 @@ 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() },)); } +fn spawn_skeleton(mut commands: Commands, mut gizmos: Gizmos) { + let hand_pose: [Posef; 26] = [ + Posef { position: Vector3f {x: -0.548, y: -0.161, z: -0.137}, orientation: Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}}, //palm + Posef { position: Vector3f {x: -0.548, y: 0.161, z: -0.137}, orientation: Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}}, + Posef { position: Vector3f {x: -0.529, y: -0.198, z: -0.126}, orientation: Quaternionf {x: -0.744, y: -0.530, z: 0.156,w: -0.376}}, + Posef { position: Vector3f {x: -0.533, y: -0.175, z: -0.090}, orientation: Quaternionf {x: -0.786, y: -0.550, z: 0.126,w: -0.254}}, + Posef { position: Vector3f {x: -0.544, y: -0.158, z: -0.069}, orientation: Quaternionf {x: -0.729, y: -0.564, z: 0.027,w: -0.387}}, + Posef { position: Vector3f {x: -0.557, y: -0.150, z: -0.065}, orientation: Quaternionf {x: -0.585, y: -0.548, z: -0.140,w: -0.582}}, + Posef { position: Vector3f {x: -0.521, y: -0.182, z: -0.136}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317,w: -0.376}}, + Posef { position: Vector3f {x: -0.550, y: -0.135, z: -0.102}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317,w: -0.376}}, + Posef { position: Vector3f {x: -0.571, y: -0.112, z: -0.082}, orientation: Quaternionf {x: -0.244, y: -0.843, z: 0.256,w: -0.404}}, + Posef { position: Vector3f {x: -0.585, y: -0.102, z: -0.070}, orientation: Quaternionf {x: -0.200, y: -0.866, z: 0.165,w: -0.428}}, + Posef { position: Vector3f {x: -0.593, y: -0.098, z: -0.064}, orientation: Quaternionf {x: -0.172, y: -0.874, z: 0.110,w: -0.440}}, + Posef { position: Vector3f {x: -0.527, y: -0.178, z: -0.144}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370,w: -0.401}}, + Posef { position: Vector3f {x: -0.559, y: -0.132, z: -0.119}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370,w: -0.401}}, + Posef { position: Vector3f {x: -0.582, y: -0.101, z: -0.104}, orientation: Quaternionf {x: -0.175, y: -0.809, z: 0.371,w: -0.420}}, + Posef { position: Vector3f {x: -0.599, y: -0.089, z: -0.092}, orientation: Quaternionf {x: -0.109, y: -0.856, z: 0.245,w: -0.443}}, + Posef { position: Vector3f {x: -0.608, y: -0.084, z: -0.086}, orientation: Quaternionf {x: -0.075, y: -0.871, z: 0.180,w: -0.450}}, + Posef { position: Vector3f {x: -0.535, y: -0.178, z: -0.152}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408,w: -0.445}}, + Posef { position: Vector3f {x: -0.568, y: -0.136, z: -0.137}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408,w: -0.445}}, + Posef { position: Vector3f {x: -0.590, y: -0.106, z: -0.130}, orientation: Quaternionf {x: -0.131, y: -0.762, z: 0.432,w: -0.464}}, + Posef { position: Vector3f {x: -0.607, y: -0.092, z: -0.122}, orientation: Quaternionf {x: -0.071, y: -0.810, z: 0.332,w: -0.477}}, + Posef { position: Vector3f {x: -0.617, y: -0.086, z: -0.117}, orientation: Quaternionf {x: -0.029, y: -0.836, z: 0.260,w: -0.482}}, + Posef { position: Vector3f {x: -0.544, y: -0.183, z: -0.159}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481,w: -0.452}}, + Posef { position: Vector3f {x: -0.576, y: -0.143, z: -0.152}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481,w: -0.452}}, + Posef { position: Vector3f {x: -0.594, y: -0.119, z: -0.154}, orientation: Quaternionf {x: -0.061, y: -0.684, z: 0.534,w: -0.493}}, + Posef { position: Vector3f {x: -0.607, y: -0.108, z: -0.152}, orientation: Quaternionf {x: 0.002, y: -0.745, z: 0.444,w: -0.498}}, + Posef { position: Vector3f {x: -0.616, y: -0.102, z: -0.150}, orientation: Quaternionf {x: 0.045, y: -0.780, z: 0.378,w: -0.496}}, + ]; + //cursed wrist math + let wrist_dist = Vec3{ x: 0.01, y: -0.05, z: 0.0 }; + let huh = Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}.to_quat(); + let why: Vec3 = huh.mul_vec3(wrist_dist); + + let offset = Vec3 { x: 0.548, y: 1.0, z: 0.137 }; + let palm = hand_pose[HandJoint::PALM]; + gizmos.sphere(palm.position.to_vec3() + offset, palm.orientation.to_quat(), 0.01, Color::WHITE); + let wrist = hand_pose[HandJoint::WRIST]; + gizmos.sphere(palm.position.to_vec3() + offset + why, palm.orientation.to_quat(), 0.01, Color::GRAY); + let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; + gizmos.sphere(thumb_meta.position.to_vec3() + offset, thumb_meta.orientation.to_quat(), 0.01, Color::RED); + let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; + gizmos.sphere(thumb_prox.position.to_vec3() + offset, thumb_prox.orientation.to_quat(), 0.008, Color::RED); + let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; + gizmos.sphere(thumb_dist.position.to_vec3() + offset, thumb_dist.orientation.to_quat(), 0.006, Color::RED); + let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; + gizmos.sphere(thumb_tip.position.to_vec3() + offset, thumb_tip.orientation.to_quat(), 0.004, Color::RED); + + let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; + gizmos.sphere(index_meta.position.to_vec3() + offset, index_meta.orientation.to_quat(), 0.01, Color::ORANGE); + let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; + gizmos.sphere(index_prox.position.to_vec3() + offset, index_prox.orientation.to_quat(), 0.008, Color::ORANGE); + let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; + gizmos.sphere(index_inter.position.to_vec3() + offset, index_inter.orientation.to_quat(), 0.006, Color::ORANGE); + let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; + gizmos.sphere(index_dist.position.to_vec3() + offset, index_dist.orientation.to_quat(), 0.004, Color::ORANGE); + let index_tip = hand_pose[HandJoint::INDEX_TIP]; + gizmos.sphere(index_tip.position.to_vec3() + offset, index_tip.orientation.to_quat(), 0.002, Color::ORANGE); + + let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; + gizmos.sphere(middle_meta.position.to_vec3() + offset, middle_meta.orientation.to_quat(), 0.01, Color::YELLOW); + let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; + gizmos.sphere(middle_prox.position.to_vec3() + offset, middle_prox.orientation.to_quat(), 0.008, Color::YELLOW); + let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; + gizmos.sphere(middle_inter.position.to_vec3() + offset, middle_inter.orientation.to_quat(), 0.006, Color::YELLOW); + let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; + gizmos.sphere(middle_dist.position.to_vec3() + offset, middle_dist.orientation.to_quat(), 0.004, Color::YELLOW); + let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; + gizmos.sphere(middle_tip.position.to_vec3() + offset, middle_tip.orientation.to_quat(), 0.002, Color::YELLOW); + + let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; + gizmos.sphere(ring_meta.position.to_vec3() + offset, ring_meta.orientation.to_quat(), 0.01, Color::GREEN); + let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; + gizmos.sphere(ring_prox.position.to_vec3() + offset, ring_prox.orientation.to_quat(), 0.008, Color::GREEN); + let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; + gizmos.sphere(ring_inter.position.to_vec3() + offset, ring_inter.orientation.to_quat(), 0.006, Color::GREEN); + let ring_dist = hand_pose[HandJoint::RING_DISTAL]; + gizmos.sphere(ring_dist.position.to_vec3() + offset, ring_dist.orientation.to_quat(), 0.004, Color::GREEN); + let ring_tip = hand_pose[HandJoint::RING_TIP]; + gizmos.sphere(ring_tip.position.to_vec3() + offset, ring_tip.orientation.to_quat(), 0.002, Color::GREEN); + + let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; + gizmos.sphere(little_meta.position.to_vec3() + offset, little_meta.orientation.to_quat(), 0.01, Color::BLUE); + let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; + gizmos.sphere(little_prox.position.to_vec3() + offset, little_prox.orientation.to_quat(), 0.008, Color::BLUE); + let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; + gizmos.sphere(little_inter.position.to_vec3() + offset, little_inter.orientation.to_quat(), 0.006, Color::BLUE); + let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; + gizmos.sphere(little_dist.position.to_vec3() + offset, little_dist.orientation.to_quat(), 0.004, Color::BLUE); + let little_tip = hand_pose[HandJoint::LITTLE_TIP]; + gizmos.sphere(little_tip.position.to_vec3() + offset, little_tip.orientation.to_quat(), 0.002, Color::BLUE); + + + + + + +} + + fn spawn_controllers_example(mut commands: Commands) { //left hand commands.spawn(( diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs new file mode 100644 index 0000000..0fcd267 --- /dev/null +++ b/src/xr_input/hand.rs @@ -0,0 +1,3 @@ +pub struct Hand { + pub HandJointPoses: [Posef; 26] +} \ No newline at end of file From b8e5f366cf3b76d90aa35db3daa7531ae334f429 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 3 Oct 2023 14:28:33 -0400 Subject: [PATCH 27/66] well this is insane but it tracks the hand now --- examples/xr.rs | 158 ++++++++++++++++++++++++++++--------------------- 1 file changed, 92 insertions(+), 66 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 80eb3b7..1f3df09 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,3 +1,5 @@ +use std::f32::consts::PI; + use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; @@ -16,13 +18,13 @@ 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) - .add_systems(Update, spawn_skeleton) + .add_systems(Update, draw_skeleton_hand) .insert_resource(PrototypeLocomotionConfig::default()) .run(); } @@ -70,103 +72,127 @@ fn setup( },)); } -fn spawn_skeleton(mut commands: Commands, mut gizmos: Gizmos) { - let hand_pose: [Posef; 26] = [ - Posef { position: Vector3f {x: -0.548, y: -0.161, z: -0.137}, orientation: Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}}, //palm - Posef { position: Vector3f {x: -0.548, y: 0.161, z: -0.137}, orientation: Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}}, - Posef { position: Vector3f {x: -0.529, y: -0.198, z: -0.126}, orientation: Quaternionf {x: -0.744, y: -0.530, z: 0.156,w: -0.376}}, - Posef { position: Vector3f {x: -0.533, y: -0.175, z: -0.090}, orientation: Quaternionf {x: -0.786, y: -0.550, z: 0.126,w: -0.254}}, - Posef { position: Vector3f {x: -0.544, y: -0.158, z: -0.069}, orientation: Quaternionf {x: -0.729, y: -0.564, z: 0.027,w: -0.387}}, - Posef { position: Vector3f {x: -0.557, y: -0.150, z: -0.065}, orientation: Quaternionf {x: -0.585, y: -0.548, z: -0.140,w: -0.582}}, - Posef { position: Vector3f {x: -0.521, y: -0.182, z: -0.136}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317,w: -0.376}}, - Posef { position: Vector3f {x: -0.550, y: -0.135, z: -0.102}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317,w: -0.376}}, - Posef { position: Vector3f {x: -0.571, y: -0.112, z: -0.082}, orientation: Quaternionf {x: -0.244, y: -0.843, z: 0.256,w: -0.404}}, - Posef { position: Vector3f {x: -0.585, y: -0.102, z: -0.070}, orientation: Quaternionf {x: -0.200, y: -0.866, z: 0.165,w: -0.428}}, - Posef { position: Vector3f {x: -0.593, y: -0.098, z: -0.064}, orientation: Quaternionf {x: -0.172, y: -0.874, z: 0.110,w: -0.440}}, - Posef { position: Vector3f {x: -0.527, y: -0.178, z: -0.144}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370,w: -0.401}}, - Posef { position: Vector3f {x: -0.559, y: -0.132, z: -0.119}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370,w: -0.401}}, - Posef { position: Vector3f {x: -0.582, y: -0.101, z: -0.104}, orientation: Quaternionf {x: -0.175, y: -0.809, z: 0.371,w: -0.420}}, - Posef { position: Vector3f {x: -0.599, y: -0.089, z: -0.092}, orientation: Quaternionf {x: -0.109, y: -0.856, z: 0.245,w: -0.443}}, - Posef { position: Vector3f {x: -0.608, y: -0.084, z: -0.086}, orientation: Quaternionf {x: -0.075, y: -0.871, z: 0.180,w: -0.450}}, - Posef { position: Vector3f {x: -0.535, y: -0.178, z: -0.152}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408,w: -0.445}}, - Posef { position: Vector3f {x: -0.568, y: -0.136, z: -0.137}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408,w: -0.445}}, - Posef { position: Vector3f {x: -0.590, y: -0.106, z: -0.130}, orientation: Quaternionf {x: -0.131, y: -0.762, z: 0.432,w: -0.464}}, - Posef { position: Vector3f {x: -0.607, y: -0.092, z: -0.122}, orientation: Quaternionf {x: -0.071, y: -0.810, z: 0.332,w: -0.477}}, - Posef { position: Vector3f {x: -0.617, y: -0.086, z: -0.117}, orientation: Quaternionf {x: -0.029, y: -0.836, z: 0.260,w: -0.482}}, - Posef { position: Vector3f {x: -0.544, y: -0.183, z: -0.159}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481,w: -0.452}}, - Posef { position: Vector3f {x: -0.576, y: -0.143, z: -0.152}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481,w: -0.452}}, - Posef { position: Vector3f {x: -0.594, y: -0.119, z: -0.154}, orientation: Quaternionf {x: -0.061, y: -0.684, z: 0.534,w: -0.493}}, - Posef { position: Vector3f {x: -0.607, y: -0.108, z: -0.152}, orientation: Quaternionf {x: 0.002, y: -0.745, z: 0.444,w: -0.498}}, - Posef { position: Vector3f {x: -0.616, y: -0.102, z: -0.150}, orientation: Quaternionf {x: 0.045, y: -0.780, z: 0.378,w: -0.496}}, - ]; - //cursed wrist math - let wrist_dist = Vec3{ x: 0.01, y: -0.05, z: 0.0 }; - let huh = Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}.to_quat(); - let why: Vec3 = huh.mul_vec3(wrist_dist); +fn draw_skeleton_hand(mut commands: Commands, + mut gizmos: Gizmos, + right_controller_query: Query<( + &GlobalTransform, + With, +)>, ) { + //draw debug for controller grip center to match palm to + let right_transform = right_controller_query.get_single().unwrap().0; + let right_translation = right_transform.compute_transform().translation; + let right_quat = right_transform.compute_transform().rotation; + gizmos.sphere(right_translation, Quat::IDENTITY, 0.01, Color::PINK); + //we need to flip this i dont know why + let flip = Quat::from_rotation_x(PI); + let flap = Quat::from_rotation_z(-45.0*(PI/180.0)); + let controller_forward = right_quat.mul_quat(flip).mul_vec3(Vec3::Y); + let controller_backward = right_quat.mul_quat(flip).mul_quat(flap); + gizmos.ray(right_translation, controller_forward, Color::PINK); - let offset = Vec3 { x: 0.548, y: 1.0, z: 0.137 }; + let hand_pose: [Posef; 26] = [ + Posef { position: Vector3f {x: -0.548, y: -0.161, z: -0.137}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, //palm + Posef { position: Vector3f {x: -0.548, y: 0.161, z: -0.137}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, + Posef { position: Vector3f {x: -0.529, y: -0.198, z: -0.126}, orientation: Quaternionf {x: -0.744, y: -0.530, z: 0.156, w: -0.376}}, + Posef { position: Vector3f {x: -0.533, y: -0.175, z: -0.090}, orientation: Quaternionf {x: -0.786, y: -0.550, z: 0.126, w: -0.254}}, + Posef { position: Vector3f {x: -0.544, y: -0.158, z: -0.069}, orientation: Quaternionf {x: -0.729, y: -0.564, z: 0.027, w: -0.387}}, + Posef { position: Vector3f {x: -0.557, y: -0.150, z: -0.065}, orientation: Quaternionf {x: -0.585, y: -0.548, z: -0.140,w: -0.582}}, + Posef { position: Vector3f {x: -0.521, y: -0.182, z: -0.136}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317, w: -0.376}}, + Posef { position: Vector3f {x: -0.550, y: -0.135, z: -0.102}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317, w: -0.376}}, + Posef { position: Vector3f {x: -0.571, y: -0.112, z: -0.082}, orientation: Quaternionf {x: -0.244, y: -0.843, z: 0.256, w: -0.404}}, + Posef { position: Vector3f {x: -0.585, y: -0.102, z: -0.070}, orientation: Quaternionf {x: -0.200, y: -0.866, z: 0.165, w: -0.428}}, + Posef { position: Vector3f {x: -0.593, y: -0.098, z: -0.064}, orientation: Quaternionf {x: -0.172, y: -0.874, z: 0.110, w: -0.440}}, + Posef { position: Vector3f {x: -0.527, y: -0.178, z: -0.144}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370, w: -0.401}}, + Posef { position: Vector3f {x: -0.559, y: -0.132, z: -0.119}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370, w: -0.401}}, + Posef { position: Vector3f {x: -0.582, y: -0.101, z: -0.104}, orientation: Quaternionf {x: -0.175, y: -0.809, z: 0.371, w: -0.420}}, + Posef { position: Vector3f {x: -0.599, y: -0.089, z: -0.092}, orientation: Quaternionf {x: -0.109, y: -0.856, z: 0.245, w: -0.443}}, + Posef { position: Vector3f {x: -0.608, y: -0.084, z: -0.086}, orientation: Quaternionf {x: -0.075, y: -0.871, z: 0.180, w: -0.450}}, + Posef { position: Vector3f {x: -0.535, y: -0.178, z: -0.152}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408, w: -0.445}}, + Posef { position: Vector3f {x: -0.568, y: -0.136, z: -0.137}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408, w: -0.445}}, + Posef { position: Vector3f {x: -0.590, y: -0.106, z: -0.130}, orientation: Quaternionf {x: -0.131, y: -0.762, z: 0.432, w: -0.464}}, + Posef { position: Vector3f {x: -0.607, y: -0.092, z: -0.122}, orientation: Quaternionf {x: -0.071, y: -0.810, z: 0.332, w: -0.477}}, + Posef { position: Vector3f {x: -0.617, y: -0.086, z: -0.117}, orientation: Quaternionf {x: -0.029, y: -0.836, z: 0.260, w: -0.482}}, + Posef { position: Vector3f {x: -0.544, y: -0.183, z: -0.159}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481, w: -0.452}}, + Posef { position: Vector3f {x: -0.576, y: -0.143, z: -0.152}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481, w: -0.452}}, + Posef { position: Vector3f {x: -0.594, y: -0.119, z: -0.154}, orientation: Quaternionf {x: -0.061, y: -0.684, z: 0.534, w: -0.493}}, + Posef { position: Vector3f {x: -0.607, y: -0.108, z: -0.152}, orientation: Quaternionf {x: 0.002, y: -0.745, z: 0.444, w: -0.498}}, + Posef { position: Vector3f {x: -0.616, y: -0.102, z: -0.150}, orientation: Quaternionf {x: 0.045, y: -0.780, z: 0.378, w: -0.496}}, + ]; + + //cursed wrist math + let wrist_dist = Vec3{ x: -0.01, y: -0.040, z: -0.015}; + + //cursed offset + let palm_negation = Vec3 { x: 0.548, y: 0.161, z: 0.137 }; + let offset = right_translation; + + //old stuff dont touch for now let palm = hand_pose[HandJoint::PALM]; - gizmos.sphere(palm.position.to_vec3() + offset, palm.orientation.to_quat(), 0.01, Color::WHITE); - let wrist = hand_pose[HandJoint::WRIST]; - gizmos.sphere(palm.position.to_vec3() + offset + why, palm.orientation.to_quat(), 0.01, Color::GRAY); + gizmos.sphere(palm.position.to_vec3() + offset + palm_negation, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); + + let rotated_wfp = palm.position.to_vec3() + right_quat.mul_quat(flip).mul_vec3(wrist_dist); + gizmos.sphere(offset + palm_negation + rotated_wfp, palm.orientation.to_quat(), 0.01, Color::GRAY); + + let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; - gizmos.sphere(thumb_meta.position.to_vec3() + offset, thumb_meta.orientation.to_quat(), 0.01, Color::RED); + draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, palm_negation, offset); + let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; - gizmos.sphere(thumb_prox.position.to_vec3() + offset, thumb_prox.orientation.to_quat(), 0.008, Color::RED); + draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, palm_negation, offset); let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; - gizmos.sphere(thumb_dist.position.to_vec3() + offset, thumb_dist.orientation.to_quat(), 0.006, Color::RED); + draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, palm_negation, offset); let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; - gizmos.sphere(thumb_tip.position.to_vec3() + offset, thumb_tip.orientation.to_quat(), 0.004, Color::RED); + draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, palm_negation, offset); let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; - gizmos.sphere(index_meta.position.to_vec3() + offset, index_meta.orientation.to_quat(), 0.01, Color::ORANGE); + draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, palm_negation, offset); let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; - gizmos.sphere(index_prox.position.to_vec3() + offset, index_prox.orientation.to_quat(), 0.008, Color::ORANGE); + draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, palm_negation, offset); let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; - gizmos.sphere(index_inter.position.to_vec3() + offset, index_inter.orientation.to_quat(), 0.006, Color::ORANGE); + draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, palm_negation, offset); let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; - gizmos.sphere(index_dist.position.to_vec3() + offset, index_dist.orientation.to_quat(), 0.004, Color::ORANGE); + draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, palm_negation, offset); let index_tip = hand_pose[HandJoint::INDEX_TIP]; - gizmos.sphere(index_tip.position.to_vec3() + offset, index_tip.orientation.to_quat(), 0.002, Color::ORANGE); + draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, palm_negation, offset); let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; - gizmos.sphere(middle_meta.position.to_vec3() + offset, middle_meta.orientation.to_quat(), 0.01, Color::YELLOW); + draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, palm_negation, offset); let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; - gizmos.sphere(middle_prox.position.to_vec3() + offset, middle_prox.orientation.to_quat(), 0.008, Color::YELLOW); + draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, palm_negation, offset); let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; - gizmos.sphere(middle_inter.position.to_vec3() + offset, middle_inter.orientation.to_quat(), 0.006, Color::YELLOW); + draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, palm_negation, offset); let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; - gizmos.sphere(middle_dist.position.to_vec3() + offset, middle_dist.orientation.to_quat(), 0.004, Color::YELLOW); + draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, palm_negation, offset); let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; - gizmos.sphere(middle_tip.position.to_vec3() + offset, middle_tip.orientation.to_quat(), 0.002, Color::YELLOW); + draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, palm_negation, offset); let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; - gizmos.sphere(ring_meta.position.to_vec3() + offset, ring_meta.orientation.to_quat(), 0.01, Color::GREEN); + draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, palm_negation, offset); let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; - gizmos.sphere(ring_prox.position.to_vec3() + offset, ring_prox.orientation.to_quat(), 0.008, Color::GREEN); + draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, palm_negation, offset); let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; - gizmos.sphere(ring_inter.position.to_vec3() + offset, ring_inter.orientation.to_quat(), 0.006, Color::GREEN); + draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, palm_negation, offset); let ring_dist = hand_pose[HandJoint::RING_DISTAL]; - gizmos.sphere(ring_dist.position.to_vec3() + offset, ring_dist.orientation.to_quat(), 0.004, Color::GREEN); + draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, palm_negation, offset); let ring_tip = hand_pose[HandJoint::RING_TIP]; - gizmos.sphere(ring_tip.position.to_vec3() + offset, ring_tip.orientation.to_quat(), 0.002, Color::GREEN); + draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, palm_negation, offset); let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; - gizmos.sphere(little_meta.position.to_vec3() + offset, little_meta.orientation.to_quat(), 0.01, Color::BLUE); + draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, palm_negation, offset); let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; - gizmos.sphere(little_prox.position.to_vec3() + offset, little_prox.orientation.to_quat(), 0.008, Color::BLUE); + draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, palm_negation, offset); let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; - gizmos.sphere(little_inter.position.to_vec3() + offset, little_inter.orientation.to_quat(), 0.006, Color::BLUE); + draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, palm_negation, offset); let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; - gizmos.sphere(little_dist.position.to_vec3() + offset, little_dist.orientation.to_quat(), 0.004, Color::BLUE); + draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, palm_negation, offset); let little_tip = hand_pose[HandJoint::LITTLE_TIP]; - gizmos.sphere(little_tip.position.to_vec3() + offset, little_tip.orientation.to_quat(), 0.002, Color::BLUE); - - - + draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, palm_negation, offset); +} +fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, palm_negation: Vec3, offset: Vec3) { + gizmos.sphere(controller_backwards.mul_vec3(joint_pos + palm_negation) + offset, joint_rot, radius, color); } From c2a147006a091df81224e10f1a441e4a9bc761f7 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 3 Oct 2023 15:03:34 -0400 Subject: [PATCH 28/66] normalized hand poses to palm --- examples/xr.rs | 157 +++++++++++++++++++++++++++++++------------------ 1 file changed, 99 insertions(+), 58 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 1f3df09..b1305da 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -82,117 +82,158 @@ fn draw_skeleton_hand(mut commands: Commands, let right_transform = right_controller_query.get_single().unwrap().0; let right_translation = right_transform.compute_transform().translation; let right_quat = right_transform.compute_transform().rotation; - gizmos.sphere(right_translation, Quat::IDENTITY, 0.01, Color::PINK); //we need to flip this i dont know why let flip = Quat::from_rotation_x(PI); let flap = Quat::from_rotation_z(-45.0*(PI/180.0)); - let controller_forward = right_quat.mul_quat(flip).mul_vec3(Vec3::Y); let controller_backward = right_quat.mul_quat(flip).mul_quat(flap); - gizmos.ray(right_translation, controller_forward, Color::PINK); let hand_pose: [Posef; 26] = [ - Posef { position: Vector3f {x: -0.548, y: -0.161, z: -0.137}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, //palm - Posef { position: Vector3f {x: -0.548, y: 0.161, z: -0.137}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, - Posef { position: Vector3f {x: -0.529, y: -0.198, z: -0.126}, orientation: Quaternionf {x: -0.744, y: -0.530, z: 0.156, w: -0.376}}, - Posef { position: Vector3f {x: -0.533, y: -0.175, z: -0.090}, orientation: Quaternionf {x: -0.786, y: -0.550, z: 0.126, w: -0.254}}, - Posef { position: Vector3f {x: -0.544, y: -0.158, z: -0.069}, orientation: Quaternionf {x: -0.729, y: -0.564, z: 0.027, w: -0.387}}, - Posef { position: Vector3f {x: -0.557, y: -0.150, z: -0.065}, orientation: Quaternionf {x: -0.585, y: -0.548, z: -0.140,w: -0.582}}, - Posef { position: Vector3f {x: -0.521, y: -0.182, z: -0.136}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317, w: -0.376}}, - Posef { position: Vector3f {x: -0.550, y: -0.135, z: -0.102}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317, w: -0.376}}, - Posef { position: Vector3f {x: -0.571, y: -0.112, z: -0.082}, orientation: Quaternionf {x: -0.244, y: -0.843, z: 0.256, w: -0.404}}, - Posef { position: Vector3f {x: -0.585, y: -0.102, z: -0.070}, orientation: Quaternionf {x: -0.200, y: -0.866, z: 0.165, w: -0.428}}, - Posef { position: Vector3f {x: -0.593, y: -0.098, z: -0.064}, orientation: Quaternionf {x: -0.172, y: -0.874, z: 0.110, w: -0.440}}, - Posef { position: Vector3f {x: -0.527, y: -0.178, z: -0.144}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370, w: -0.401}}, - Posef { position: Vector3f {x: -0.559, y: -0.132, z: -0.119}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370, w: -0.401}}, - Posef { position: Vector3f {x: -0.582, y: -0.101, z: -0.104}, orientation: Quaternionf {x: -0.175, y: -0.809, z: 0.371, w: -0.420}}, - Posef { position: Vector3f {x: -0.599, y: -0.089, z: -0.092}, orientation: Quaternionf {x: -0.109, y: -0.856, z: 0.245, w: -0.443}}, - Posef { position: Vector3f {x: -0.608, y: -0.084, z: -0.086}, orientation: Quaternionf {x: -0.075, y: -0.871, z: 0.180, w: -0.450}}, - Posef { position: Vector3f {x: -0.535, y: -0.178, z: -0.152}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408, w: -0.445}}, - Posef { position: Vector3f {x: -0.568, y: -0.136, z: -0.137}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408, w: -0.445}}, - Posef { position: Vector3f {x: -0.590, y: -0.106, z: -0.130}, orientation: Quaternionf {x: -0.131, y: -0.762, z: 0.432, w: -0.464}}, - Posef { position: Vector3f {x: -0.607, y: -0.092, z: -0.122}, orientation: Quaternionf {x: -0.071, y: -0.810, z: 0.332, w: -0.477}}, - Posef { position: Vector3f {x: -0.617, y: -0.086, z: -0.117}, orientation: Quaternionf {x: -0.029, y: -0.836, z: 0.260, w: -0.482}}, - Posef { position: Vector3f {x: -0.544, y: -0.183, z: -0.159}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481, w: -0.452}}, - Posef { position: Vector3f {x: -0.576, y: -0.143, z: -0.152}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481, w: -0.452}}, - Posef { position: Vector3f {x: -0.594, y: -0.119, z: -0.154}, orientation: Quaternionf {x: -0.061, y: -0.684, z: 0.534, w: -0.493}}, - Posef { position: Vector3f {x: -0.607, y: -0.108, z: -0.152}, orientation: Quaternionf {x: 0.002, y: -0.745, z: 0.444, w: -0.498}}, - Posef { position: Vector3f {x: -0.616, y: -0.102, z: -0.150}, orientation: Quaternionf {x: 0.045, y: -0.780, z: 0.378, w: -0.496}}, + 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.0, y: 0.32, z: 0.0}, 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}}, ]; + //log_hand(hand_pose); + + //cursed wrist math let wrist_dist = Vec3{ x: -0.01, y: -0.040, z: -0.015}; //cursed offset - let palm_negation = Vec3 { x: 0.548, y: 0.161, z: 0.137 }; let offset = right_translation; //old stuff dont touch for now let palm = hand_pose[HandJoint::PALM]; - gizmos.sphere(palm.position.to_vec3() + offset + palm_negation, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); + gizmos.sphere(palm.position.to_vec3() + offset, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); let rotated_wfp = palm.position.to_vec3() + right_quat.mul_quat(flip).mul_vec3(wrist_dist); - gizmos.sphere(offset + palm_negation + rotated_wfp, palm.orientation.to_quat(), 0.01, Color::GRAY); + gizmos.sphere(offset + rotated_wfp, palm.orientation.to_quat(), 0.01, Color::GRAY); let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; - draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, offset); let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; - draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, offset); let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; - draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, offset); let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; - draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, offset); let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; - draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, offset); let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; - draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, offset); let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; - draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, offset); let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; - draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, offset); let index_tip = hand_pose[HandJoint::INDEX_TIP]; - draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, offset); let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; - draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, offset); let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; - draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, offset); let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; - draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, offset); let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; - draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, offset); let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; - draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, offset); let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; - draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, offset); let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; - draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, offset); let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; - draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, offset); let ring_dist = hand_pose[HandJoint::RING_DISTAL]; - draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, offset); let ring_tip = hand_pose[HandJoint::RING_TIP]; - draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, offset); let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; - draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, offset); let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; - draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, offset); let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; - draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, offset); let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; - draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, offset); let little_tip = hand_pose[HandJoint::LITTLE_TIP]; - draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, offset); + + } -fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, palm_negation: Vec3, offset: Vec3) { - gizmos.sphere(controller_backwards.mul_vec3(joint_pos + palm_negation) + offset, joint_rot, radius, color); +fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, offset: Vec3) { + gizmos.sphere(controller_backwards.mul_vec3(joint_pos) + offset, joint_rot, radius, color); +} + +fn log_hand(hand_pose: [Posef; 26]) { + let palm_vec = hand_pose[HandJoint::PALM].position.to_vec3(); + info!("palm: {}", hand_pose[HandJoint::PALM].position.to_vec3() - palm_vec); + info!("wrist: {}", hand_pose[HandJoint::WRIST].position.to_vec3() - palm_vec); + + info!("tm: {}", hand_pose[HandJoint::THUMB_METACARPAL].position.to_vec3() - palm_vec); + info!("tp: {}", hand_pose[HandJoint::THUMB_PROXIMAL].position.to_vec3() - palm_vec); + info!("td: {}", hand_pose[HandJoint::THUMB_DISTAL].position.to_vec3() - palm_vec); + info!("tt: {}", hand_pose[HandJoint::THUMB_TIP].position.to_vec3() - palm_vec); + + info!("im: {}", hand_pose[HandJoint::INDEX_METACARPAL].position.to_vec3() - palm_vec); + info!("ip: {}", hand_pose[HandJoint::INDEX_PROXIMAL].position.to_vec3() - palm_vec); + info!("ii: {}", hand_pose[HandJoint::INDEX_INTERMEDIATE].position.to_vec3() - palm_vec); + info!("id: {}", hand_pose[HandJoint::INDEX_DISTAL].position.to_vec3() - palm_vec); + info!("it: {}", hand_pose[HandJoint::INDEX_TIP].position.to_vec3() - palm_vec); + + info!("mm: {}", hand_pose[HandJoint::MIDDLE_METACARPAL].position.to_vec3() - palm_vec); + info!("mp: {}", hand_pose[HandJoint::MIDDLE_PROXIMAL].position.to_vec3() - palm_vec); + info!("mi: {}", hand_pose[HandJoint::MIDDLE_INTERMEDIATE].position.to_vec3() - palm_vec); + info!("md: {}", hand_pose[HandJoint::MIDDLE_DISTAL].position.to_vec3() - palm_vec); + info!("mt: {}", hand_pose[HandJoint::MIDDLE_TIP].position.to_vec3() - palm_vec); + + info!("rm: {}", hand_pose[HandJoint::RING_METACARPAL].position.to_vec3() - palm_vec); + info!("rp: {}", hand_pose[HandJoint::RING_PROXIMAL].position.to_vec3() - palm_vec); + info!("ri: {}", hand_pose[HandJoint::RING_INTERMEDIATE].position.to_vec3() - palm_vec); + info!("rd: {}", hand_pose[HandJoint::RING_DISTAL].position.to_vec3() - palm_vec); + info!("rt: {}", hand_pose[HandJoint::RING_TIP].position.to_vec3() - palm_vec); + + info!("lm: {}", hand_pose[HandJoint::LITTLE_METACARPAL].position.to_vec3() - palm_vec); + info!("lp: {}", hand_pose[HandJoint::LITTLE_PROXIMAL].position.to_vec3() - palm_vec); + info!("li: {}", hand_pose[HandJoint::LITTLE_INTERMEDIATE].position.to_vec3() - palm_vec); + info!("ld: {}", hand_pose[HandJoint::LITTLE_DISTAL].position.to_vec3() - palm_vec); + info!("lt: {}", hand_pose[HandJoint::LITTLE_TIP].position.to_vec3() - palm_vec); } From a940778405569b045343cbf466818c5f4bac142b Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 3 Oct 2023 15:47:18 -0400 Subject: [PATCH 29/66] both hands work --- examples/xr.rs | 117 ++++++++++++++++++++++++++++--------------------- 1 file changed, 68 insertions(+), 49 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index b1305da..3d8058b 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,9 +1,10 @@ use std::f32::consts::PI; +use std::ops::Mul; use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; -use bevy_openxr::xr_input::{Vec3Conv, QuatConv}; +use bevy_openxr::xr_input::{Vec3Conv, QuatConv, Hand}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ @@ -24,7 +25,7 @@ fn main() { .add_systems(Startup, setup) .add_systems(Update, proto_locomotion) .add_systems(Startup, spawn_controllers_example) - .add_systems(Update, draw_skeleton_hand) + .add_systems(Update, draw_skeleton_hands) .insert_resource(PrototypeLocomotionConfig::default()) .run(); } @@ -72,24 +73,39 @@ fn setup( },)); } -fn draw_skeleton_hand(mut commands: Commands, +fn draw_skeleton_hands(mut commands: Commands, mut gizmos: Gizmos, right_controller_query: Query<( &GlobalTransform, With, -)>, ) { +)>, +left_controller_query: Query<( + &GlobalTransform, + With, +)>,) { + + let left_hand_transform = left_controller_query.get_single().unwrap().0.compute_transform(); + draw_hand(&mut gizmos, left_hand_transform, Hand::Left); + let right_hand_transform = right_controller_query.get_single().unwrap().0.compute_transform(); + draw_hand(&mut gizmos, right_hand_transform, Hand::Right) + + + +} + +fn draw_hand(mut gizmos: &mut Gizmos, + controller_transform: Transform, +hand: Hand) { //draw debug for controller grip center to match palm to - let right_transform = right_controller_query.get_single().unwrap().0; - let right_translation = right_transform.compute_transform().translation; - let right_quat = right_transform.compute_transform().rotation; + let hand_translation = controller_transform.translation; + let hand_quat = controller_transform.rotation; //we need to flip this i dont know why let flip = Quat::from_rotation_x(PI); - let flap = Quat::from_rotation_z(-45.0*(PI/180.0)); - let controller_backward = right_quat.mul_quat(flip).mul_quat(flap); + let controller_backward = hand_quat.mul_quat(flip); - let hand_pose: [Posef; 26] = [ + let test: [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.0, y: 0.32, z: 0.0}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, + 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}}, @@ -121,80 +137,83 @@ fn draw_skeleton_hand(mut commands: Commands, 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}}, ]; + let hand_pose = flip_hand_pose(test.clone(), hand); + //log_hand(hand_pose); - - //cursed wrist math - let wrist_dist = Vec3{ x: -0.01, y: -0.040, z: -0.015}; - - //cursed offset - let offset = right_translation; - - //old stuff dont touch for now let palm = hand_pose[HandJoint::PALM]; - gizmos.sphere(palm.position.to_vec3() + offset, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); + gizmos.sphere(palm.position.to_vec3() + hand_translation, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); - let rotated_wfp = palm.position.to_vec3() + right_quat.mul_quat(flip).mul_vec3(wrist_dist); - gizmos.sphere(offset + rotated_wfp, palm.orientation.to_quat(), 0.01, Color::GRAY); + let wrist = hand_pose[HandJoint::WRIST]; + draw_joint(&mut gizmos, wrist.position.to_vec3(), wrist.orientation.to_quat(), 0.01, Color::GRAY, controller_backward, hand_translation); let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; - draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, offset); + draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, hand_translation); let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; - draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, offset); + draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, hand_translation); let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; - draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, offset); + draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, hand_translation); let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; - draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, offset); + draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, hand_translation); let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; - draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, hand_translation); let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; - draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, hand_translation); let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; - draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, hand_translation); let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; - draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, hand_translation); let index_tip = hand_pose[HandJoint::INDEX_TIP]; - draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, hand_translation); let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; - draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, hand_translation); let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; - draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, hand_translation); let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; - draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, hand_translation); let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; - draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, hand_translation); let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; - draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, hand_translation); let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; - draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, hand_translation); let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; - draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, hand_translation); let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; - draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, hand_translation); let ring_dist = hand_pose[HandJoint::RING_DISTAL]; - draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, hand_translation); let ring_tip = hand_pose[HandJoint::RING_TIP]; - draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, hand_translation); let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; - draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, offset); + draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, hand_translation); let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; - draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, offset); + draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, hand_translation); let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; - draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, offset); + draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, hand_translation); let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; - draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, offset); + draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, hand_translation); let little_tip = hand_pose[HandJoint::LITTLE_TIP]; - draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, offset); - - - + draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, hand_translation); +} +fn flip_hand_pose(hand_pose: [Posef; 26], hand: Hand) -> [Posef; 26] { + let mut new_pose = hand_pose; + match hand { + Hand::Left => { + for pose in new_pose.iter_mut() { + pose.position.x = -pose.position.x; + } + }, + Hand::Right => (), + } + return new_pose; } fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, offset: Vec3) { From cda06c9dd22eb04e72d0593b235a32d25f940066 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sat, 7 Oct 2023 17:05:47 -0400 Subject: [PATCH 30/66] left hand works too --- examples/xr.rs | 844 ++++++++++++++++++++++++++++++------- src/xr_input/hand_poses.rs | 520 +++++++++++++++++++++++ src/xr_input/mod.rs | 1 + 3 files changed, 1224 insertions(+), 141 deletions(-) create mode 100644 src/xr_input/hand_poses.rs diff --git a/examples/xr.rs b/examples/xr.rs index 3d8058b..d34c4b7 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -2,16 +2,18 @@ use std::f32::consts::PI; use std::ops::Mul; use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; -use bevy::prelude::*; +use bevy::math::vec3; use bevy::transform::components::Transform; -use bevy_openxr::xr_input::{Vec3Conv, QuatConv, Hand}; +use bevy::{gizmos, prelude::*}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; +use bevy_openxr::xr_input::hand_poses::*; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, }; +use bevy_openxr::xr_input::{Hand, QuatConv, Vec3Conv}; use bevy_openxr::DefaultXrPlugins; -use openxr::{Posef, Quaternionf, Vector3f, HandJoint}; +use openxr::{HandJoint, Posef, Quaternionf, Vector3f}; fn main() { color_eyre::install().unwrap(); @@ -68,34 +70,306 @@ fn setup( }); // camera commands.spawn((Camera3dBundle { - transform: Transform::from_xyz(0.25, 1.25, 0.0).looking_at(Vec3{x: -0.548, y: -0.161, z: -0.137}, 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() },)); } -fn draw_skeleton_hands(mut commands: Commands, +fn draw_skeleton_hands( + mut commands: Commands, mut gizmos: Gizmos, - right_controller_query: Query<( - &GlobalTransform, - With, -)>, -left_controller_query: Query<( - &GlobalTransform, - With, -)>,) { - - let left_hand_transform = left_controller_query.get_single().unwrap().0.compute_transform(); - draw_hand(&mut gizmos, left_hand_transform, Hand::Left); - let right_hand_transform = right_controller_query.get_single().unwrap().0.compute_transform(); - draw_hand(&mut gizmos, right_hand_transform, Hand::Right) - - - + right_controller_query: Query<(&GlobalTransform, With)>, + left_controller_query: Query<(&GlobalTransform, With)>, +) { + let left_hand_transform = left_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left); + let right_hand_transform = right_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); + draw_hand_bones(&mut gizmos, right_hand_transform, Hand::Right); } -fn draw_hand(mut gizmos: &mut Gizmos, - controller_transform: Transform, -hand: Hand) { +fn pose_array_to_transform_array(hand_pose: [Posef; 26]) -> [Transform; 26] { + let mut result_array: [Transform; 26] = [Transform::default(); 26]; + for (place, data) in result_array.iter_mut().zip(hand_pose.iter()) { + *place = Transform { + translation: data.position.to_vec3(), + rotation: data.orientation.to_quat(), + scale: Vec3::splat(1.0), + } + } + return result_array; +} + +fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: 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, + }; + //get paml 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); + //draw debug rays + gizmos.ray( + hand_translation, + palm_quat.mul_vec3(Vec3::Z * 0.2), + Color::BLUE, + ); + gizmos.ray( + hand_translation, + palm_quat.mul_vec3(Vec3::Y * 0.2), + Color::GREEN, + ); + gizmos.ray( + hand_translation, + palm_quat.mul_vec3(Vec3::X * 0.2), + Color::RED, + ); + //get simulated bones + let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); + //draw controller-palm bone(should be zero length) + let palm = hand_transform_array[HandJoint::PALM]; + gizmos.ray(hand_translation, palm.translation, Color::WHITE); + //draw palm-wrist + let wrist = hand_transform_array[HandJoint::WRIST]; + gizmos.ray( + hand_translation + palm.translation, + palm_quat.mul_vec3(wrist.translation), + Color::GRAY, + ); + + //thumb + //better finger drawing? + 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 color = Color::RED; + let splay = Quat::from_rotation_y(splay_direction * 30.0 * PI / 180.0); + let splay_quat = palm_quat.mul_quat(splay); + for bone in thumb_joints.iter() { + match prior_start { + Some(start) => { + let tp_lrot = Quat::from_rotation_y(splay_direction * 5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } + + //better finger drawing? + 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 color = Color::ORANGE; + 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 tp_lrot = Quat::from_rotation_x(-5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } + + //better finger drawing? + 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 color = Color::YELLOW; + 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 tp_lrot = Quat::from_rotation_x(-5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } + //better finger drawing? + 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 color = Color::GREEN; + 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 tp_lrot = Quat::from_rotation_x(-5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } + + //better finger drawing? + 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 color = Color::BLUE; + 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 tp_lrot = Quat::from_rotation_x(-5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } +} + +fn draw_hand(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: Hand) { //draw debug for controller grip center to match palm to let hand_translation = controller_transform.translation; let hand_quat = controller_transform.rotation; @@ -103,104 +377,280 @@ hand: Hand) { let flip = Quat::from_rotation_x(PI); let controller_backward = hand_quat.mul_quat(flip); - let test: [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}}, + let test_hand_pose = get_test_hand_pose_array(); - 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}}, + let hand_pose = flip_hand_pose(test_hand_pose.clone(), hand); - 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}}, + // let hand_transform_array: [Transform; 26] = pose_array_to_transform_array(hand_pose); + let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); - 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}}, + let palm = hand_transform_array[HandJoint::PALM]; + gizmos.sphere( + palm.translation + hand_translation, + palm.rotation.mul_quat(controller_backward), + 0.01, + Color::WHITE, + ); - 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}}, + let wrist = hand_transform_array[HandJoint::WRIST]; + draw_joint( + &mut gizmos, + wrist.translation, + wrist.rotation, + 0.01, + Color::GRAY, + controller_backward, + hand_translation, + ); - 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}}, - ]; + let thumb_meta = hand_transform_array[HandJoint::THUMB_METACARPAL]; + draw_joint( + &mut gizmos, + thumb_meta.translation, + thumb_meta.rotation, + 0.01, + Color::RED, + controller_backward, + hand_translation, + ); - let hand_pose = flip_hand_pose(test.clone(), hand); + let thumb_prox = hand_transform_array[HandJoint::THUMB_PROXIMAL]; + draw_joint( + &mut gizmos, + thumb_prox.translation, + thumb_prox.rotation, + 0.008, + Color::RED, + controller_backward, + hand_translation, + ); + let thumb_dist = hand_transform_array[HandJoint::THUMB_DISTAL]; + draw_joint( + &mut gizmos, + thumb_dist.translation, + thumb_dist.rotation, + 0.006, + Color::RED, + controller_backward, + hand_translation, + ); + let thumb_tip = hand_transform_array[HandJoint::THUMB_TIP]; + draw_joint( + &mut gizmos, + thumb_tip.translation, + thumb_tip.rotation, + 0.004, + Color::RED, + controller_backward, + hand_translation, + ); - //log_hand(hand_pose); + let index_meta = hand_transform_array[HandJoint::INDEX_METACARPAL]; + draw_joint( + &mut gizmos, + index_meta.translation, + index_meta.rotation, + 0.01, + Color::ORANGE, + controller_backward, + hand_translation, + ); + let index_prox = hand_transform_array[HandJoint::INDEX_PROXIMAL]; + draw_joint( + &mut gizmos, + index_prox.translation, + index_prox.rotation, + 0.008, + Color::ORANGE, + controller_backward, + hand_translation, + ); - let palm = hand_pose[HandJoint::PALM]; - gizmos.sphere(palm.position.to_vec3() + hand_translation, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); + let index_inter = hand_transform_array[HandJoint::INDEX_INTERMEDIATE]; + draw_joint( + &mut gizmos, + index_inter.translation, + index_inter.rotation, + 0.006, + Color::ORANGE, + controller_backward, + hand_translation, + ); - let wrist = hand_pose[HandJoint::WRIST]; - draw_joint(&mut gizmos, wrist.position.to_vec3(), wrist.orientation.to_quat(), 0.01, Color::GRAY, controller_backward, hand_translation); + let index_dist = hand_transform_array[HandJoint::INDEX_DISTAL]; + draw_joint( + &mut gizmos, + index_dist.translation, + index_dist.rotation, + 0.004, + Color::ORANGE, + controller_backward, + hand_translation, + ); + let index_tip = hand_transform_array[HandJoint::INDEX_TIP]; + draw_joint( + &mut gizmos, + index_tip.translation, + index_tip.rotation, + 0.002, + Color::ORANGE, + controller_backward, + hand_translation, + ); - let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; - draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, hand_translation); + let middle_meta = hand_transform_array[HandJoint::MIDDLE_METACARPAL]; + draw_joint( + &mut gizmos, + middle_meta.translation, + middle_meta.rotation, + 0.01, + Color::YELLOW, + controller_backward, + hand_translation, + ); + let middle_prox = hand_transform_array[HandJoint::MIDDLE_PROXIMAL]; + draw_joint( + &mut gizmos, + middle_prox.translation, + middle_prox.rotation, + 0.008, + Color::YELLOW, + controller_backward, + hand_translation, + ); + let middle_inter = hand_transform_array[HandJoint::MIDDLE_INTERMEDIATE]; + draw_joint( + &mut gizmos, + middle_inter.translation, + middle_inter.rotation, + 0.006, + Color::YELLOW, + controller_backward, + hand_translation, + ); + let middle_dist = hand_transform_array[HandJoint::MIDDLE_DISTAL]; + draw_joint( + &mut gizmos, + middle_dist.translation, + middle_dist.rotation, + 0.004, + Color::YELLOW, + controller_backward, + hand_translation, + ); + let middle_tip = hand_transform_array[HandJoint::MIDDLE_TIP]; + draw_joint( + &mut gizmos, + middle_tip.translation, + middle_tip.rotation, + 0.002, + Color::YELLOW, + controller_backward, + hand_translation, + ); - let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; - draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, hand_translation); - let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; - draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, hand_translation); - let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; - draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, hand_translation); + let ring_meta = hand_transform_array[HandJoint::RING_METACARPAL]; + draw_joint( + &mut gizmos, + ring_meta.translation, + ring_meta.rotation, + 0.01, + Color::GREEN, + controller_backward, + hand_translation, + ); + let ring_prox = hand_transform_array[HandJoint::RING_PROXIMAL]; + draw_joint( + &mut gizmos, + ring_prox.translation, + ring_prox.rotation, + 0.008, + Color::GREEN, + controller_backward, + hand_translation, + ); + let ring_inter = hand_transform_array[HandJoint::RING_INTERMEDIATE]; + draw_joint( + &mut gizmos, + ring_inter.translation, + ring_inter.rotation, + 0.006, + Color::GREEN, + controller_backward, + hand_translation, + ); + let ring_dist = hand_transform_array[HandJoint::RING_DISTAL]; + draw_joint( + &mut gizmos, + ring_dist.translation, + ring_dist.rotation, + 0.004, + Color::GREEN, + controller_backward, + hand_translation, + ); + let ring_tip = hand_transform_array[HandJoint::RING_TIP]; + draw_joint( + &mut gizmos, + ring_tip.translation, + ring_tip.rotation, + 0.002, + Color::GREEN, + controller_backward, + hand_translation, + ); - let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; - draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, hand_translation); - let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; - draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, hand_translation); - let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; - draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, hand_translation); - let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; - draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, hand_translation); - let index_tip = hand_pose[HandJoint::INDEX_TIP]; - draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, hand_translation); - - let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; - draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, hand_translation); - let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; - draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, hand_translation); - let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; - draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, hand_translation); - let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; - draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, hand_translation); - let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; - draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, hand_translation); - - let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; - draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, hand_translation); - let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; - draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, hand_translation); - let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; - draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, hand_translation); - let ring_dist = hand_pose[HandJoint::RING_DISTAL]; - draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, hand_translation); - let ring_tip = hand_pose[HandJoint::RING_TIP]; - draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, hand_translation); - - let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; - draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, hand_translation); - let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; - draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, hand_translation); - let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; - draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, hand_translation); - let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; - draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, hand_translation); - let little_tip = hand_pose[HandJoint::LITTLE_TIP]; - draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, hand_translation); + let little_meta = hand_transform_array[HandJoint::LITTLE_METACARPAL]; + draw_joint( + &mut gizmos, + little_meta.translation, + little_meta.rotation, + 0.01, + Color::BLUE, + controller_backward, + hand_translation, + ); + let little_prox = hand_transform_array[HandJoint::LITTLE_PROXIMAL]; + draw_joint( + &mut gizmos, + little_prox.translation, + little_prox.rotation, + 0.008, + Color::BLUE, + controller_backward, + hand_translation, + ); + let little_inter = hand_transform_array[HandJoint::LITTLE_INTERMEDIATE]; + draw_joint( + &mut gizmos, + little_inter.translation, + little_inter.rotation, + 0.006, + Color::BLUE, + controller_backward, + hand_translation, + ); + let little_dist = hand_transform_array[HandJoint::LITTLE_DISTAL]; + draw_joint( + &mut gizmos, + little_dist.translation, + little_dist.rotation, + 0.004, + Color::BLUE, + controller_backward, + hand_translation, + ); + let little_tip = hand_transform_array[HandJoint::LITTLE_TIP]; + draw_joint( + &mut gizmos, + little_tip.translation, + little_tip.rotation, + 0.002, + Color::BLUE, + controller_backward, + hand_translation, + ); } fn flip_hand_pose(hand_pose: [Posef; 26], hand: Hand) -> [Posef; 26] { @@ -210,52 +660,164 @@ fn flip_hand_pose(hand_pose: [Posef; 26], hand: Hand) -> [Posef; 26] { for pose in new_pose.iter_mut() { pose.position.x = -pose.position.x; } - }, + } Hand::Right => (), } return new_pose; } -fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, offset: Vec3) { - gizmos.sphere(controller_backwards.mul_vec3(joint_pos) + offset, joint_rot, radius, color); +fn draw_joint( + gizmos: &mut Gizmos, + joint_pos: Vec3, + joint_rot: Quat, + radius: f32, + color: Color, + controller_backwards: Quat, + offset: Vec3, +) { + gizmos.sphere( + controller_backwards.mul_vec3(joint_pos) + offset, + joint_rot, + radius, + color, + ); } fn log_hand(hand_pose: [Posef; 26]) { - let palm_vec = hand_pose[HandJoint::PALM].position.to_vec3(); - info!("palm: {}", hand_pose[HandJoint::PALM].position.to_vec3() - palm_vec); - info!("wrist: {}", hand_pose[HandJoint::WRIST].position.to_vec3() - palm_vec); + 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!("tm: {}", hand_pose[HandJoint::THUMB_METACARPAL].position.to_vec3() - palm_vec); - info!("tp: {}", hand_pose[HandJoint::THUMB_PROXIMAL].position.to_vec3() - palm_vec); - info!("td: {}", hand_pose[HandJoint::THUMB_DISTAL].position.to_vec3() - palm_vec); - info!("tt: {}", hand_pose[HandJoint::THUMB_TIP].position.to_vec3() - palm_vec); - - info!("im: {}", hand_pose[HandJoint::INDEX_METACARPAL].position.to_vec3() - palm_vec); - info!("ip: {}", hand_pose[HandJoint::INDEX_PROXIMAL].position.to_vec3() - palm_vec); - info!("ii: {}", hand_pose[HandJoint::INDEX_INTERMEDIATE].position.to_vec3() - palm_vec); - info!("id: {}", hand_pose[HandJoint::INDEX_DISTAL].position.to_vec3() - palm_vec); - info!("it: {}", hand_pose[HandJoint::INDEX_TIP].position.to_vec3() - palm_vec); + 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!("mm: {}", hand_pose[HandJoint::MIDDLE_METACARPAL].position.to_vec3() - palm_vec); - info!("mp: {}", hand_pose[HandJoint::MIDDLE_PROXIMAL].position.to_vec3() - palm_vec); - info!("mi: {}", hand_pose[HandJoint::MIDDLE_INTERMEDIATE].position.to_vec3() - palm_vec); - info!("md: {}", hand_pose[HandJoint::MIDDLE_DISTAL].position.to_vec3() - palm_vec); - info!("mt: {}", hand_pose[HandJoint::MIDDLE_TIP].position.to_vec3() - palm_vec); + 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!("rm: {}", hand_pose[HandJoint::RING_METACARPAL].position.to_vec3() - palm_vec); - info!("rp: {}", hand_pose[HandJoint::RING_PROXIMAL].position.to_vec3() - palm_vec); - info!("ri: {}", hand_pose[HandJoint::RING_INTERMEDIATE].position.to_vec3() - palm_vec); - info!("rd: {}", hand_pose[HandJoint::RING_DISTAL].position.to_vec3() - palm_vec); - info!("rt: {}", hand_pose[HandJoint::RING_TIP].position.to_vec3() - palm_vec); + 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!("lm: {}", hand_pose[HandJoint::LITTLE_METACARPAL].position.to_vec3() - palm_vec); - info!("lp: {}", hand_pose[HandJoint::LITTLE_PROXIMAL].position.to_vec3() - palm_vec); - info!("li: {}", hand_pose[HandJoint::LITTLE_INTERMEDIATE].position.to_vec3() - palm_vec); - info!("ld: {}", hand_pose[HandJoint::LITTLE_DISTAL].position.to_vec3() - palm_vec); - info!("lt: {}", hand_pose[HandJoint::LITTLE_TIP].position.to_vec3() - palm_vec); + 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() + ); } - fn spawn_controllers_example(mut commands: Commands) { //left hand commands.spawn(( 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/mod.rs b/src/xr_input/mod.rs index 55e7c56..1ee3e71 100644 --- a/src/xr_input/mod.rs +++ b/src/xr_input/mod.rs @@ -4,6 +4,7 @@ pub mod oculus_touch; pub mod prototype_locomotion; pub mod trackers; pub mod xr_camera; +pub mod hand_poses; use crate::resources::XrSession; use crate::xr_begin_frame; From 2a6f4cddc17593d2a61571f9f7895192a43aa580 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sat, 7 Oct 2023 18:38:25 -0400 Subject: [PATCH 31/66] hand state resource is used to drive skeleton --- examples/xr.rs | 318 +++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 283 insertions(+), 35 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index d34c4b7..7f65bc4 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -5,8 +5,11 @@ use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::math::vec3; use bevy::transform::components::Transform; use bevy::{gizmos, prelude::*}; +use bevy_openxr::input::XrInput; +use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; use bevy_openxr::xr_input::hand_poses::*; +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, @@ -28,7 +31,9 @@ fn main() { .add_systems(Update, proto_locomotion) .add_systems(Startup, spawn_controllers_example) .add_systems(Update, draw_skeleton_hands) + .add_systems(PreUpdate, update_hand_states) .insert_resource(PrototypeLocomotionConfig::default()) + .insert_resource(HandStatesResource::default()) .run(); } @@ -82,25 +87,144 @@ fn setup( },)); } +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"), + } +} + fn draw_skeleton_hands( mut commands: Commands, mut gizmos: Gizmos, right_controller_query: Query<(&GlobalTransform, With)>, left_controller_query: Query<(&GlobalTransform, With)>, + hand_states_option: Option>, ) { - let left_hand_transform = left_controller_query - .get_single() - .unwrap() - .0 - .compute_transform(); - draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left); - let right_hand_transform = right_controller_query - .get_single() - .unwrap() - .0 - .compute_transform(); - // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); - draw_hand_bones(&mut gizmos, right_hand_transform, Hand::Right); + match hand_states_option { + Some(hands) => { + let left_hand_transform = left_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left, hands.left); + let right_hand_transform = right_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); + draw_hand_bones(&mut gizmos, right_hand_transform, Hand::Right, hands.right); + } + None => info!("hand states resource not initialized yet"), + } } fn pose_array_to_transform_array(hand_pose: [Posef; 26]) -> [Transform; 26] { @@ -114,8 +238,108 @@ fn pose_array_to_transform_array(hand_pose: [Posef; 26]) -> [Transform; 26] { } return result_array; } +#[derive(Clone, Copy)] +pub enum ButtonState { + OFF, + TOUCHED, + PRESSED, +} -fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: Hand) { +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 { + grip: f32, + trigger_state: TriggerState, + a_button: ButtonState, + b_button: ButtonState, + thumbstick: ThumbstickState, +} + +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(), + } + } +} + +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; + } +} + +fn draw_hand_bones( + mut gizmos: &mut Gizmos, + controller_transform: Transform, + hand: Hand, + hand_state: HandState, +) { let left_hand_rot = Quat::from_rotation_y(180.0 * PI / 180.0); let hand_translation: Vec3 = match hand { Hand::Left => controller_transform.translation, @@ -131,26 +355,30 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han Hand::Left => -1.0, Hand::Right => 1.0, }; + //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 paml 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); //draw debug rays - gizmos.ray( - hand_translation, - palm_quat.mul_vec3(Vec3::Z * 0.2), - Color::BLUE, - ); - gizmos.ray( - hand_translation, - palm_quat.mul_vec3(Vec3::Y * 0.2), - Color::GREEN, - ); - gizmos.ray( - hand_translation, - palm_quat.mul_vec3(Vec3::X * 0.2), - Color::RED, - ); + // gizmos.ray( + // hand_translation, + // palm_quat.mul_vec3(Vec3::Z * 0.2), + // Color::BLUE, + // ); + // gizmos.ray( + // hand_translation, + // palm_quat.mul_vec3(Vec3::Y * 0.2), + // Color::GREEN, + // ); + // gizmos.ray( + // hand_translation, + // palm_quat.mul_vec3(Vec3::X * 0.2), + // Color::RED, + // ); //get simulated bones let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); //draw controller-palm bone(should be zero length) @@ -177,11 +405,13 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han let mut prior_vector: Option = None; let color = Color::RED; let splay = Quat::from_rotation_y(splay_direction * 30.0 * PI / 180.0); - let splay_quat = palm_quat.mul_quat(splay); + 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 tp_lrot = Quat::from_rotation_y(splay_direction * 5.0 * PI / 180.0); + 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(); @@ -205,6 +435,7 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han } } + //index //better finger drawing? let thumb_joints = [ HandJoint::INDEX_METACARPAL, @@ -222,7 +453,8 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han for bone in thumb_joints.iter() { match prior_start { Some(start) => { - let tp_lrot = Quat::from_rotation_x(-5.0 * PI / 180.0); + 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(); @@ -263,7 +495,8 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han for bone in thumb_joints.iter() { match prior_start { Some(start) => { - let tp_lrot = Quat::from_rotation_x(-5.0 * PI / 180.0); + 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(); @@ -303,7 +536,8 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han for bone in thumb_joints.iter() { match prior_start { Some(start) => { - let tp_lrot = Quat::from_rotation_x(-5.0 * PI / 180.0); + 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(); @@ -344,7 +578,8 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han for bone in thumb_joints.iter() { match prior_start { Some(start) => { - let tp_lrot = Quat::from_rotation_x(-5.0 * PI / 180.0); + 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(); @@ -369,6 +604,19 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han } } +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 draw_hand(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: Hand) { //draw debug for controller grip center to match palm to let hand_translation = controller_transform.translation; From 2fd4364b10d363965aefa5dbec48f4122a2472dc Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 00:11:23 -0400 Subject: [PATCH 32/66] spawn bones and update --- examples/xr.rs | 556 ++++++++++++++++++-------------------------- src/xr_input/mod.rs | 4 +- 2 files changed, 231 insertions(+), 329 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 7f65bc4..23de2bd 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -32,6 +32,8 @@ fn main() { .add_systems(Startup, spawn_controllers_example) .add_systems(Update, draw_skeleton_hands) .add_systems(PreUpdate, update_hand_states) + .add_systems(PostUpdate, draw_hand_entities) + .add_systems(Startup, spawn_hand_entities) .insert_resource(PrototypeLocomotionConfig::default()) .insert_resource(HandStatesResource::default()) .run(); @@ -87,6 +89,49 @@ fn setup( },)); } +pub fn spawn_hand_entities(mut commands: Commands) { + let hands = [Hand::Left, Hand::Right]; + let bones = [ + HandBone::PALM, + HandBone::WRIST, + HandBone::THUMB_METACARPAL, + HandBone::THUMB_PROXIMAL, + HandBone::THUMB_DISTAL, + HandBone::THUMB_TIP, + HandBone::INDEX_METACARPAL, + HandBone::INDEX_PROXIMAL, + HandBone::INDEX_INTERMEDIATE, + HandBone::INDEX_DISTAL, + HandBone::INDEX_TIP, + HandBone::MIDDLE_METACARPAL, + HandBone::MIDDLE_PROXIMAL, + HandBone::MIDDLE_INTERMEDIATE, + HandBone::MIDDLE_DISTAL, + HandBone::MIDDLE_TIP, + HandBone::RING_METACARPAL, + HandBone::RING_PROXIMAL, + HandBone::RING_INTERMEDIATE, + HandBone::RING_DISTAL, + HandBone::RING_TIP, + HandBone::LITTLE_METACARPAL, + HandBone::LITTLE_PROXIMAL, + HandBone::LITTLE_INTERMEDIATE, + HandBone::LITTLE_DISTAL, + HandBone::LITTLE_TIP, + ]; + + for hand in hands.iter() { + for bone in bones.iter() { + commands.spawn(( + SpatialBundle::default(), + bone.clone(), + OpenXRTracker, + hand.clone(), + )); + } + } +} + pub fn update_hand_states( oculus_controller: Res, hand_states_option: Option>, @@ -201,11 +246,11 @@ pub fn update_hand_states( } fn draw_skeleton_hands( - mut commands: Commands, mut gizmos: Gizmos, 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)>, ) { match hand_states_option { Some(hands) => { @@ -214,30 +259,61 @@ fn draw_skeleton_hands( .unwrap() .0 .compute_transform(); - draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left, hands.left); + draw_hand_bones( + &mut gizmos, + left_hand_transform, + Hand::Left, + hands.left, + &mut hand_bone_query, + ); let right_hand_transform = right_controller_query .get_single() .unwrap() .0 .compute_transform(); // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); - draw_hand_bones(&mut gizmos, right_hand_transform, Hand::Right, hands.right); + draw_hand_bones( + &mut gizmos, + right_hand_transform, + Hand::Right, + hands.right, + &mut hand_bone_query, + ); } None => info!("hand states resource not initialized yet"), } } -fn pose_array_to_transform_array(hand_pose: [Posef; 26]) -> [Transform; 26] { - let mut result_array: [Transform; 26] = [Transform::default(); 26]; - for (place, data) in result_array.iter_mut().zip(hand_pose.iter()) { - *place = Transform { - translation: data.position.to_vec3(), - rotation: data.orientation.to_quat(), - scale: Vec3::splat(1.0), - } - } - return result_array; +#[derive(Component, Debug, Clone, Copy)] +pub enum HandBone { + PALM, + WRIST, + THUMB_METACARPAL, + THUMB_PROXIMAL, + THUMB_DISTAL, + THUMB_TIP, + INDEX_METACARPAL, + INDEX_PROXIMAL, + INDEX_INTERMEDIATE, + INDEX_DISTAL, + INDEX_TIP, + MIDDLE_METACARPAL, + MIDDLE_PROXIMAL, + MIDDLE_INTERMEDIATE, + MIDDLE_DISTAL, + MIDDLE_TIP, + RING_METACARPAL, + RING_PROXIMAL, + RING_INTERMEDIATE, + RING_DISTAL, + RING_TIP, + LITTLE_METACARPAL, + LITTLE_PROXIMAL, + LITTLE_INTERMEDIATE, + LITTLE_DISTAL, + LITTLE_TIP, } + #[derive(Clone, Copy)] pub enum ButtonState { OFF, @@ -334,11 +410,50 @@ impl HandState { } } +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::THUMB_METACARPAL => (0.01, Color::RED), + HandBone::THUMB_PROXIMAL => (0.008, Color::RED), + HandBone::THUMB_DISTAL => (0.006, Color::RED), + HandBone::THUMB_TIP => (0.004, Color::RED), + HandBone::INDEX_METACARPAL => (0.01, Color::ORANGE), + HandBone::INDEX_PROXIMAL => (0.008, Color::ORANGE), + HandBone::INDEX_INTERMEDIATE => (0.006, Color::ORANGE), + HandBone::INDEX_DISTAL => (0.004, Color::ORANGE), + HandBone::INDEX_TIP => (0.002, Color::ORANGE), + HandBone::MIDDLE_METACARPAL => (0.01, Color::YELLOW), + HandBone::MIDDLE_PROXIMAL => (0.008, Color::YELLOW), + HandBone::MIDDLE_INTERMEDIATE => (0.006, Color::YELLOW), + HandBone::MIDDLE_DISTAL => (0.004, Color::YELLOW), + HandBone::MIDDLE_TIP => (0.002, Color::YELLOW), + HandBone::RING_METACARPAL => (0.01, Color::GREEN), + HandBone::RING_PROXIMAL => (0.008, Color::GREEN), + HandBone::RING_INTERMEDIATE => (0.006, Color::GREEN), + HandBone::RING_DISTAL => (0.004, Color::GREEN), + HandBone::RING_TIP => (0.002, Color::GREEN), + HandBone::LITTLE_METACARPAL => (0.01, Color::BLUE), + HandBone::LITTLE_PROXIMAL => (0.008, Color::BLUE), + HandBone::LITTLE_INTERMEDIATE => (0.006, Color::BLUE), + HandBone::LITTLE_DISTAL => (0.004, Color::BLUE), + HandBone::LITTLE_TIP => (0.002, Color::BLUE), + } +} + fn draw_hand_bones( mut gizmos: &mut Gizmos, 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 { @@ -355,6 +470,9 @@ fn draw_hand_bones( 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(); @@ -384,6 +502,10 @@ fn draw_hand_bones( //draw controller-palm bone(should be zero length) let palm = hand_transform_array[HandJoint::PALM]; gizmos.ray(hand_translation, palm.translation, Color::WHITE); + calc_transforms[HandJoint::PALM] = Transform { + translation: hand_translation + palm.translation, + ..default() + }; //draw palm-wrist let wrist = hand_transform_array[HandJoint::WRIST]; gizmos.ray( @@ -391,6 +513,10 @@ fn draw_hand_bones( palm_quat.mul_vec3(wrist.translation), Color::GRAY, ); + calc_transforms[HandJoint::WRIST] = Transform { + translation: hand_translation + palm.translation + palm_quat.mul_vec3(wrist.translation), + ..default() + }; //thumb //better finger drawing? @@ -420,6 +546,11 @@ fn draw_hand_bones( 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]; @@ -431,6 +562,11 @@ fn draw_hand_bones( 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() + }; } } } @@ -463,6 +599,11 @@ fn draw_hand_bones( 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]; @@ -474,6 +615,11 @@ fn draw_hand_bones( 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() + }; } } } @@ -505,6 +651,11 @@ fn draw_hand_bones( 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]; @@ -516,6 +667,11 @@ fn draw_hand_bones( 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() + }; } } } @@ -546,6 +702,11 @@ fn draw_hand_bones( 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]; @@ -557,6 +718,11 @@ fn draw_hand_bones( 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() + }; } } } @@ -588,6 +754,11 @@ fn draw_hand_bones( 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]; @@ -599,9 +770,54 @@ fn draw_hand_bones( 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::THUMB_METACARPAL => HandJoint::THUMB_METACARPAL, + HandBone::THUMB_PROXIMAL => HandJoint::THUMB_PROXIMAL, + HandBone::THUMB_DISTAL => HandJoint::THUMB_DISTAL, + HandBone::THUMB_TIP => HandJoint::THUMB_TIP, + HandBone::INDEX_METACARPAL => HandJoint::INDEX_METACARPAL, + HandBone::INDEX_PROXIMAL => HandJoint::INDEX_PROXIMAL, + HandBone::INDEX_INTERMEDIATE => HandJoint::INDEX_INTERMEDIATE, + HandBone::INDEX_DISTAL => HandJoint::INDEX_DISTAL, + HandBone::INDEX_TIP => HandJoint::INDEX_TIP, + HandBone::MIDDLE_METACARPAL => HandJoint::MIDDLE_METACARPAL, + HandBone::MIDDLE_PROXIMAL => HandJoint::MIDDLE_PROXIMAL, + HandBone::MIDDLE_INTERMEDIATE => HandJoint::MIDDLE_INTERMEDIATE, + HandBone::MIDDLE_DISTAL => HandJoint::MIDDLE_DISTAL, + HandBone::MIDDLE_TIP => HandJoint::MIDDLE_TIP, + HandBone::RING_METACARPAL => HandJoint::RING_METACARPAL, + HandBone::RING_PROXIMAL => HandJoint::RING_PROXIMAL, + HandBone::RING_INTERMEDIATE => HandJoint::RING_INTERMEDIATE, + HandBone::RING_DISTAL => HandJoint::RING_DISTAL, + HandBone::RING_TIP => HandJoint::RING_TIP, + HandBone::LITTLE_METACARPAL => HandJoint::LITTLE_METACARPAL, + HandBone::LITTLE_PROXIMAL => HandJoint::LITTLE_PROXIMAL, + HandBone::LITTLE_INTERMEDIATE => HandJoint::LITTLE_INTERMEDIATE, + HandBone::LITTLE_DISTAL => HandJoint::LITTLE_DISTAL, + HandBone::LITTLE_TIP => HandJoint::LITTLE_TIP, + } } fn get_bone_curl_angle(bone: HandJoint, curl: f32) -> f32 { @@ -617,320 +833,6 @@ fn get_bone_curl_angle(bone: HandJoint, curl: f32) -> f32 { return curl_angle; } -fn draw_hand(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: Hand) { - //draw debug for controller grip center to match palm to - let hand_translation = controller_transform.translation; - let hand_quat = controller_transform.rotation; - //we need to flip this i dont know why - let flip = Quat::from_rotation_x(PI); - let controller_backward = hand_quat.mul_quat(flip); - - let test_hand_pose = get_test_hand_pose_array(); - - let hand_pose = flip_hand_pose(test_hand_pose.clone(), hand); - - // let hand_transform_array: [Transform; 26] = pose_array_to_transform_array(hand_pose); - let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); - - let palm = hand_transform_array[HandJoint::PALM]; - gizmos.sphere( - palm.translation + hand_translation, - palm.rotation.mul_quat(controller_backward), - 0.01, - Color::WHITE, - ); - - let wrist = hand_transform_array[HandJoint::WRIST]; - draw_joint( - &mut gizmos, - wrist.translation, - wrist.rotation, - 0.01, - Color::GRAY, - controller_backward, - hand_translation, - ); - - let thumb_meta = hand_transform_array[HandJoint::THUMB_METACARPAL]; - draw_joint( - &mut gizmos, - thumb_meta.translation, - thumb_meta.rotation, - 0.01, - Color::RED, - controller_backward, - hand_translation, - ); - - let thumb_prox = hand_transform_array[HandJoint::THUMB_PROXIMAL]; - draw_joint( - &mut gizmos, - thumb_prox.translation, - thumb_prox.rotation, - 0.008, - Color::RED, - controller_backward, - hand_translation, - ); - let thumb_dist = hand_transform_array[HandJoint::THUMB_DISTAL]; - draw_joint( - &mut gizmos, - thumb_dist.translation, - thumb_dist.rotation, - 0.006, - Color::RED, - controller_backward, - hand_translation, - ); - let thumb_tip = hand_transform_array[HandJoint::THUMB_TIP]; - draw_joint( - &mut gizmos, - thumb_tip.translation, - thumb_tip.rotation, - 0.004, - Color::RED, - controller_backward, - hand_translation, - ); - - let index_meta = hand_transform_array[HandJoint::INDEX_METACARPAL]; - draw_joint( - &mut gizmos, - index_meta.translation, - index_meta.rotation, - 0.01, - Color::ORANGE, - controller_backward, - hand_translation, - ); - let index_prox = hand_transform_array[HandJoint::INDEX_PROXIMAL]; - draw_joint( - &mut gizmos, - index_prox.translation, - index_prox.rotation, - 0.008, - Color::ORANGE, - controller_backward, - hand_translation, - ); - - let index_inter = hand_transform_array[HandJoint::INDEX_INTERMEDIATE]; - draw_joint( - &mut gizmos, - index_inter.translation, - index_inter.rotation, - 0.006, - Color::ORANGE, - controller_backward, - hand_translation, - ); - - let index_dist = hand_transform_array[HandJoint::INDEX_DISTAL]; - draw_joint( - &mut gizmos, - index_dist.translation, - index_dist.rotation, - 0.004, - Color::ORANGE, - controller_backward, - hand_translation, - ); - - let index_tip = hand_transform_array[HandJoint::INDEX_TIP]; - draw_joint( - &mut gizmos, - index_tip.translation, - index_tip.rotation, - 0.002, - Color::ORANGE, - controller_backward, - hand_translation, - ); - - let middle_meta = hand_transform_array[HandJoint::MIDDLE_METACARPAL]; - draw_joint( - &mut gizmos, - middle_meta.translation, - middle_meta.rotation, - 0.01, - Color::YELLOW, - controller_backward, - hand_translation, - ); - let middle_prox = hand_transform_array[HandJoint::MIDDLE_PROXIMAL]; - draw_joint( - &mut gizmos, - middle_prox.translation, - middle_prox.rotation, - 0.008, - Color::YELLOW, - controller_backward, - hand_translation, - ); - let middle_inter = hand_transform_array[HandJoint::MIDDLE_INTERMEDIATE]; - draw_joint( - &mut gizmos, - middle_inter.translation, - middle_inter.rotation, - 0.006, - Color::YELLOW, - controller_backward, - hand_translation, - ); - let middle_dist = hand_transform_array[HandJoint::MIDDLE_DISTAL]; - draw_joint( - &mut gizmos, - middle_dist.translation, - middle_dist.rotation, - 0.004, - Color::YELLOW, - controller_backward, - hand_translation, - ); - let middle_tip = hand_transform_array[HandJoint::MIDDLE_TIP]; - draw_joint( - &mut gizmos, - middle_tip.translation, - middle_tip.rotation, - 0.002, - Color::YELLOW, - controller_backward, - hand_translation, - ); - - let ring_meta = hand_transform_array[HandJoint::RING_METACARPAL]; - draw_joint( - &mut gizmos, - ring_meta.translation, - ring_meta.rotation, - 0.01, - Color::GREEN, - controller_backward, - hand_translation, - ); - let ring_prox = hand_transform_array[HandJoint::RING_PROXIMAL]; - draw_joint( - &mut gizmos, - ring_prox.translation, - ring_prox.rotation, - 0.008, - Color::GREEN, - controller_backward, - hand_translation, - ); - let ring_inter = hand_transform_array[HandJoint::RING_INTERMEDIATE]; - draw_joint( - &mut gizmos, - ring_inter.translation, - ring_inter.rotation, - 0.006, - Color::GREEN, - controller_backward, - hand_translation, - ); - let ring_dist = hand_transform_array[HandJoint::RING_DISTAL]; - draw_joint( - &mut gizmos, - ring_dist.translation, - ring_dist.rotation, - 0.004, - Color::GREEN, - controller_backward, - hand_translation, - ); - let ring_tip = hand_transform_array[HandJoint::RING_TIP]; - draw_joint( - &mut gizmos, - ring_tip.translation, - ring_tip.rotation, - 0.002, - Color::GREEN, - controller_backward, - hand_translation, - ); - - let little_meta = hand_transform_array[HandJoint::LITTLE_METACARPAL]; - draw_joint( - &mut gizmos, - little_meta.translation, - little_meta.rotation, - 0.01, - Color::BLUE, - controller_backward, - hand_translation, - ); - let little_prox = hand_transform_array[HandJoint::LITTLE_PROXIMAL]; - draw_joint( - &mut gizmos, - little_prox.translation, - little_prox.rotation, - 0.008, - Color::BLUE, - controller_backward, - hand_translation, - ); - let little_inter = hand_transform_array[HandJoint::LITTLE_INTERMEDIATE]; - draw_joint( - &mut gizmos, - little_inter.translation, - little_inter.rotation, - 0.006, - Color::BLUE, - controller_backward, - hand_translation, - ); - let little_dist = hand_transform_array[HandJoint::LITTLE_DISTAL]; - draw_joint( - &mut gizmos, - little_dist.translation, - little_dist.rotation, - 0.004, - Color::BLUE, - controller_backward, - hand_translation, - ); - let little_tip = hand_transform_array[HandJoint::LITTLE_TIP]; - draw_joint( - &mut gizmos, - little_tip.translation, - little_tip.rotation, - 0.002, - Color::BLUE, - controller_backward, - hand_translation, - ); -} - -fn flip_hand_pose(hand_pose: [Posef; 26], hand: Hand) -> [Posef; 26] { - let mut new_pose = hand_pose; - match hand { - Hand::Left => { - for pose in new_pose.iter_mut() { - pose.position.x = -pose.position.x; - } - } - Hand::Right => (), - } - return new_pose; -} - -fn draw_joint( - gizmos: &mut Gizmos, - joint_pos: Vec3, - joint_rot: Quat, - radius: f32, - color: Color, - controller_backwards: Quat, - offset: Vec3, -) { - gizmos.sphere( - controller_backwards.mul_vec3(joint_pos) + offset, - joint_rot, - radius, - color, - ); -} - fn log_hand(hand_pose: [Posef; 26]) { let palm_wrist = hand_pose[HandJoint::WRIST].position.to_vec3() - hand_pose[HandJoint::PALM].position.to_vec3(); diff --git a/src/xr_input/mod.rs b/src/xr_input/mod.rs index 1ee3e71..a81badc 100644 --- a/src/xr_input/mod.rs +++ b/src/xr_input/mod.rs @@ -13,7 +13,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}; @@ -28,7 +28,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, From 3388b1a6c1abd8731ea09988e1c9b080cb554828 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 00:16:14 -0400 Subject: [PATCH 33/66] slight cleaning --- examples/xr.rs | 66 +++++++++----------------------------------------- 1 file changed, 11 insertions(+), 55 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 23de2bd..91f6587 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -30,7 +30,7 @@ fn main() { .add_systems(Startup, setup) .add_systems(Update, proto_locomotion) .add_systems(Startup, spawn_controllers_example) - .add_systems(Update, draw_skeleton_hands) + .add_systems(Update, update_emulated_hand_skeletons) .add_systems(PreUpdate, update_hand_states) .add_systems(PostUpdate, draw_hand_entities) .add_systems(Startup, spawn_hand_entities) @@ -245,8 +245,7 @@ pub fn update_hand_states( } } -fn draw_skeleton_hands( - mut gizmos: Gizmos, +fn update_emulated_hand_skeletons( right_controller_query: Query<(&GlobalTransform, With)>, left_controller_query: Query<(&GlobalTransform, With)>, hand_states_option: Option>, @@ -259,8 +258,7 @@ fn draw_skeleton_hands( .unwrap() .0 .compute_transform(); - draw_hand_bones( - &mut gizmos, + update_hand_bones_emulated( left_hand_transform, Hand::Left, hands.left, @@ -271,9 +269,7 @@ fn draw_skeleton_hands( .unwrap() .0 .compute_transform(); - // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); - draw_hand_bones( - &mut gizmos, + update_hand_bones_emulated( right_hand_transform, Hand::Right, hands.right, @@ -448,8 +444,7 @@ fn get_bone_gizmo_style(hand_bone: &HandBone) -> (f32, Color) { } } -fn draw_hand_bones( - mut gizmos: &mut Gizmos, +fn update_hand_bones_emulated( controller_transform: Transform, hand: Hand, hand_state: HandState, @@ -477,49 +472,26 @@ fn draw_hand_bones( let grip_curl = hand_state.grip; let index_curl = hand_state.get_index_curl(); let thumb_curl = hand_state.get_thumb_curl(); - //get paml quat + //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); - //draw debug rays - // gizmos.ray( - // hand_translation, - // palm_quat.mul_vec3(Vec3::Z * 0.2), - // Color::BLUE, - // ); - // gizmos.ray( - // hand_translation, - // palm_quat.mul_vec3(Vec3::Y * 0.2), - // Color::GREEN, - // ); - // gizmos.ray( - // hand_translation, - // palm_quat.mul_vec3(Vec3::X * 0.2), - // Color::RED, - // ); //get simulated bones let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); - //draw controller-palm bone(should be zero length) + //palm let palm = hand_transform_array[HandJoint::PALM]; - gizmos.ray(hand_translation, palm.translation, Color::WHITE); calc_transforms[HandJoint::PALM] = Transform { translation: hand_translation + palm.translation, ..default() }; - //draw palm-wrist + //wrist let wrist = hand_transform_array[HandJoint::WRIST]; - gizmos.ray( - hand_translation + palm.translation, - palm_quat.mul_vec3(wrist.translation), - Color::GRAY, - ); calc_transforms[HandJoint::WRIST] = Transform { translation: hand_translation + palm.translation + palm_quat.mul_vec3(wrist.translation), ..default() }; //thumb - //better finger drawing? let thumb_joints = [ HandJoint::THUMB_METACARPAL, HandJoint::THUMB_PROXIMAL, @@ -529,7 +501,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::RED; 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); @@ -542,7 +513,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -558,7 +528,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); @@ -572,7 +541,6 @@ fn draw_hand_bones( } //index - //better finger drawing? let thumb_joints = [ HandJoint::INDEX_METACARPAL, HandJoint::INDEX_PROXIMAL, @@ -583,7 +551,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::ORANGE; 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() { @@ -595,7 +562,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -611,7 +577,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); @@ -624,7 +589,7 @@ fn draw_hand_bones( } } - //better finger drawing? + //middle let thumb_joints = [ HandJoint::MIDDLE_METACARPAL, HandJoint::MIDDLE_PROXIMAL, @@ -635,7 +600,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::YELLOW; 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() { @@ -647,7 +611,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -663,7 +626,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); @@ -675,7 +637,7 @@ fn draw_hand_bones( } } } - //better finger drawing? + //ring let thumb_joints = [ HandJoint::RING_METACARPAL, HandJoint::RING_PROXIMAL, @@ -686,7 +648,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::GREEN; 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() { @@ -698,7 +659,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -714,7 +674,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); @@ -727,7 +686,7 @@ fn draw_hand_bones( } } - //better finger drawing? + //little let thumb_joints = [ HandJoint::LITTLE_METACARPAL, HandJoint::LITTLE_PROXIMAL, @@ -738,7 +697,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::BLUE; 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() { @@ -750,7 +708,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -766,7 +723,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); From 03a37afe4e03fbf76ef3b6c8dd4a0b3a8df076c8 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 00:17:31 -0400 Subject: [PATCH 34/66] cargo fix --- examples/xr.rs | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 91f6587..ad738a9 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,22 +1,22 @@ use std::f32::consts::PI; -use std::ops::Mul; + use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; -use bevy::math::vec3; + use bevy::transform::components::Transform; -use bevy::{gizmos, prelude::*}; +use bevy::{prelude::*}; use bevy_openxr::input::XrInput; use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; -use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; + use bevy_openxr::xr_input::hand_poses::*; 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, }; -use bevy_openxr::xr_input::{Hand, QuatConv, Vec3Conv}; +use bevy_openxr::xr_input::{Hand, Vec3Conv}; use bevy_openxr::DefaultXrPlugins; -use openxr::{HandJoint, Posef, Quaternionf, Vector3f}; +use openxr::{HandJoint, Posef}; fn main() { color_eyre::install().unwrap(); @@ -790,7 +790,7 @@ fn get_bone_curl_angle(bone: HandJoint, curl: f32) -> f32 { } fn log_hand(hand_pose: [Posef; 26]) { - let palm_wrist = hand_pose[HandJoint::WRIST].position.to_vec3() + let _palm_wrist = hand_pose[HandJoint::WRIST].position.to_vec3() - hand_pose[HandJoint::PALM].position.to_vec3(); info!( "palm-wrist: {}", From 7931eb511b405669a00fabced47ca4e0d6b8133a Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 00:19:43 -0400 Subject: [PATCH 35/66] mass rename --- examples/xr.rs | 210 ++++++++++++++++++++++++------------------------- 1 file changed, 105 insertions(+), 105 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index ad738a9..d47b6f8 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -4,7 +4,7 @@ use std::f32::consts::PI; use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::transform::components::Transform; -use bevy::{prelude::*}; +use bevy::prelude::*; use bevy_openxr::input::XrInput; use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; @@ -92,32 +92,32 @@ fn setup( pub fn spawn_hand_entities(mut commands: Commands) { let hands = [Hand::Left, Hand::Right]; let bones = [ - HandBone::PALM, - HandBone::WRIST, - HandBone::THUMB_METACARPAL, - HandBone::THUMB_PROXIMAL, - HandBone::THUMB_DISTAL, - HandBone::THUMB_TIP, - HandBone::INDEX_METACARPAL, - HandBone::INDEX_PROXIMAL, - HandBone::INDEX_INTERMEDIATE, - HandBone::INDEX_DISTAL, - HandBone::INDEX_TIP, - HandBone::MIDDLE_METACARPAL, - HandBone::MIDDLE_PROXIMAL, - HandBone::MIDDLE_INTERMEDIATE, - HandBone::MIDDLE_DISTAL, - HandBone::MIDDLE_TIP, - HandBone::RING_METACARPAL, - HandBone::RING_PROXIMAL, - HandBone::RING_INTERMEDIATE, - HandBone::RING_DISTAL, - HandBone::RING_TIP, - HandBone::LITTLE_METACARPAL, - HandBone::LITTLE_PROXIMAL, - HandBone::LITTLE_INTERMEDIATE, - HandBone::LITTLE_DISTAL, - HandBone::LITTLE_TIP, + 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, ]; for hand in hands.iter() { @@ -282,32 +282,32 @@ fn update_emulated_hand_skeletons( #[derive(Component, Debug, Clone, Copy)] pub enum HandBone { - PALM, - WRIST, - THUMB_METACARPAL, - THUMB_PROXIMAL, - THUMB_DISTAL, - THUMB_TIP, - INDEX_METACARPAL, - INDEX_PROXIMAL, - INDEX_INTERMEDIATE, - INDEX_DISTAL, - INDEX_TIP, - MIDDLE_METACARPAL, - MIDDLE_PROXIMAL, - MIDDLE_INTERMEDIATE, - MIDDLE_DISTAL, - MIDDLE_TIP, - RING_METACARPAL, - RING_PROXIMAL, - RING_INTERMEDIATE, - RING_DISTAL, - RING_TIP, - LITTLE_METACARPAL, - LITTLE_PROXIMAL, - LITTLE_INTERMEDIATE, - LITTLE_DISTAL, - LITTLE_TIP, + 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, } #[derive(Clone, Copy)] @@ -415,32 +415,32 @@ fn draw_hand_entities(mut gizmos: Gizmos, query: Query<(&Transform, &HandBone)>) 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::THUMB_METACARPAL => (0.01, Color::RED), - HandBone::THUMB_PROXIMAL => (0.008, Color::RED), - HandBone::THUMB_DISTAL => (0.006, Color::RED), - HandBone::THUMB_TIP => (0.004, Color::RED), - HandBone::INDEX_METACARPAL => (0.01, Color::ORANGE), - HandBone::INDEX_PROXIMAL => (0.008, Color::ORANGE), - HandBone::INDEX_INTERMEDIATE => (0.006, Color::ORANGE), - HandBone::INDEX_DISTAL => (0.004, Color::ORANGE), - HandBone::INDEX_TIP => (0.002, Color::ORANGE), - HandBone::MIDDLE_METACARPAL => (0.01, Color::YELLOW), - HandBone::MIDDLE_PROXIMAL => (0.008, Color::YELLOW), - HandBone::MIDDLE_INTERMEDIATE => (0.006, Color::YELLOW), - HandBone::MIDDLE_DISTAL => (0.004, Color::YELLOW), - HandBone::MIDDLE_TIP => (0.002, Color::YELLOW), - HandBone::RING_METACARPAL => (0.01, Color::GREEN), - HandBone::RING_PROXIMAL => (0.008, Color::GREEN), - HandBone::RING_INTERMEDIATE => (0.006, Color::GREEN), - HandBone::RING_DISTAL => (0.004, Color::GREEN), - HandBone::RING_TIP => (0.002, Color::GREEN), - HandBone::LITTLE_METACARPAL => (0.01, Color::BLUE), - HandBone::LITTLE_PROXIMAL => (0.008, Color::BLUE), - HandBone::LITTLE_INTERMEDIATE => (0.006, Color::BLUE), - HandBone::LITTLE_DISTAL => (0.004, Color::BLUE), - HandBone::LITTLE_TIP => (0.002, Color::BLUE), + 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), } } @@ -747,32 +747,32 @@ fn update_hand_bones_emulated( fn match_index(handbone: &HandBone) -> HandJoint { match handbone { - HandBone::PALM => HandJoint::PALM, - HandBone::WRIST => HandJoint::WRIST, - HandBone::THUMB_METACARPAL => HandJoint::THUMB_METACARPAL, - HandBone::THUMB_PROXIMAL => HandJoint::THUMB_PROXIMAL, - HandBone::THUMB_DISTAL => HandJoint::THUMB_DISTAL, - HandBone::THUMB_TIP => HandJoint::THUMB_TIP, - HandBone::INDEX_METACARPAL => HandJoint::INDEX_METACARPAL, - HandBone::INDEX_PROXIMAL => HandJoint::INDEX_PROXIMAL, - HandBone::INDEX_INTERMEDIATE => HandJoint::INDEX_INTERMEDIATE, - HandBone::INDEX_DISTAL => HandJoint::INDEX_DISTAL, - HandBone::INDEX_TIP => HandJoint::INDEX_TIP, - HandBone::MIDDLE_METACARPAL => HandJoint::MIDDLE_METACARPAL, - HandBone::MIDDLE_PROXIMAL => HandJoint::MIDDLE_PROXIMAL, - HandBone::MIDDLE_INTERMEDIATE => HandJoint::MIDDLE_INTERMEDIATE, - HandBone::MIDDLE_DISTAL => HandJoint::MIDDLE_DISTAL, - HandBone::MIDDLE_TIP => HandJoint::MIDDLE_TIP, - HandBone::RING_METACARPAL => HandJoint::RING_METACARPAL, - HandBone::RING_PROXIMAL => HandJoint::RING_PROXIMAL, - HandBone::RING_INTERMEDIATE => HandJoint::RING_INTERMEDIATE, - HandBone::RING_DISTAL => HandJoint::RING_DISTAL, - HandBone::RING_TIP => HandJoint::RING_TIP, - HandBone::LITTLE_METACARPAL => HandJoint::LITTLE_METACARPAL, - HandBone::LITTLE_PROXIMAL => HandJoint::LITTLE_PROXIMAL, - HandBone::LITTLE_INTERMEDIATE => HandJoint::LITTLE_INTERMEDIATE, - HandBone::LITTLE_DISTAL => HandJoint::LITTLE_DISTAL, - HandBone::LITTLE_TIP => HandJoint::LITTLE_TIP, + 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, } } From 567408daf5b079fa9050f3588f099fe3c3bb559f Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 21:04:20 -0400 Subject: [PATCH 36/66] create resource to help with entity selection --- examples/xr.rs | 80 +++++++++++++++++++++++++++++++++++----- src/xr_input/hand.rs | 88 ++++++++++++++++++++++++++++++++++++++++++-- src/xr_input/mod.rs | 1 + 3 files changed, 157 insertions(+), 12 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index d47b6f8..4fb94bb 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,13 +1,13 @@ use std::f32::consts::PI; - use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; -use bevy::transform::components::Transform; use bevy::prelude::*; +use bevy::transform::components::Transform; use bevy_openxr::input::XrInput; use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; +use bevy_openxr::xr_input::hand::HandsResource; use bevy_openxr::xr_input::hand_poses::*; use bevy_openxr::xr_input::oculus_touch::OculusController; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; @@ -119,17 +119,79 @@ pub fn spawn_hand_entities(mut commands: Commands) { HandBone::LittleDistal, HandBone::LittleTip, ]; - + //hand resource + let mut hand_resource = HandsResource { ..default() }; for hand in hands.iter() { for bone in bones.iter() { - commands.spawn(( - SpatialBundle::default(), - bone.clone(), - OpenXRTracker, - hand.clone(), - )); + 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); } pub fn update_hand_states( diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs index 0fcd267..b84e0dc 100644 --- a/src/xr_input/hand.rs +++ b/src/xr_input/hand.rs @@ -1,3 +1,85 @@ -pub struct Hand { - pub HandJointPoses: [Posef; 26] -} \ No newline at end of file +use bevy::prelude::{Entity, Resource}; + +#[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 } + } +} diff --git a/src/xr_input/mod.rs b/src/xr_input/mod.rs index a81badc..65eef19 100644 --- a/src/xr_input/mod.rs +++ b/src/xr_input/mod.rs @@ -5,6 +5,7 @@ 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; From 37a6298c390134318cf93c4d7bca2f4b07e57a35 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 10 Oct 2023 13:05:09 -0400 Subject: [PATCH 37/66] moved everything to the hand file --- examples/xr.rs | 909 +------------------------------------------ src/xr_input/hand.rs | 907 +++++++++++++++++++++++++++++++++++++++++- 2 files changed, 909 insertions(+), 907 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 4fb94bb..cbc7ac7 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,22 +1,16 @@ -use std::f32::consts::PI; + use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; -use bevy_openxr::input::XrInput; -use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; -use bevy_openxr::xr_input::hand::HandsResource; -use bevy_openxr::xr_input::hand_poses::*; -use bevy_openxr::xr_input::oculus_touch::OculusController; +use bevy_openxr::xr_input::hand::{spawn_hand_entities, update_hand_states, HandStatesResource, update_emulated_hand_skeletons, draw_hand_entities}; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, }; -use bevy_openxr::xr_input::{Hand, Vec3Conv}; use bevy_openxr::DefaultXrPlugins; -use openxr::{HandJoint, Posef}; fn main() { color_eyre::install().unwrap(); @@ -89,903 +83,6 @@ fn setup( },)); } -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); -} - -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"), - } -} - -fn update_emulated_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)>, -) { - 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"), - } -} - -#[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, -} - -#[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 { - grip: f32, - trigger_state: TriggerState, - a_button: ButtonState, - b_button: ButtonState, - thumbstick: ThumbstickState, -} - -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(), - } - } -} - -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; - } -} - -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), - } -} - -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() - ); -} - fn spawn_controllers_example(mut commands: Commands) { //left hand commands.spawn(( @@ -1001,4 +98,4 @@ fn spawn_controllers_example(mut commands: Commands) { OpenXRTracker, SpatialBundle::default(), )); -} +} \ No newline at end of file diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs index b84e0dc..f02b5c4 100644 --- a/src/xr_input/hand.rs +++ b/src/xr_input/hand.rs @@ -1,4 +1,11 @@ -use bevy::prelude::{Entity, Resource}; +use std::f32::consts::PI; + +use bevy::prelude::{Entity, Resource, Component, default, SpatialBundle, Commands, Res, ResMut, info, Transform, Query, Quat, Vec3, GlobalTransform, With, Gizmos, Color}; +use openxr::{HandJoint, Posef}; + +use crate::{resources::{XrFrameState, XrInstance, XrSession}, input::XrInput, xr_input::Vec3Conv}; + +use super::{trackers::{OpenXRTracker, OpenXRRightController, OpenXRLeftController}, Hand, oculus_touch::OculusController, hand_poses::get_simulated_open_hand_transforms}; #[derive(Resource, Default)] pub struct HandsResource { @@ -83,3 +90,901 @@ impl Default for LittleResource { 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_emulated_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)>, +) { + 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"), + } +} + +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), + } +} \ No newline at end of file From 901257fdc2aefdaa4796405a4c9b4bdbe57d0d4a Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sat, 14 Oct 2023 11:02:49 -0400 Subject: [PATCH 38/66] look upon my bad code --- examples/xr.rs | 10 +-- src/xr_input/hand.rs | 174 +++++++++++++++++++++++++++++++++---------- 2 files changed, 138 insertions(+), 46 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index cbc7ac7..363cbe9 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -5,7 +5,7 @@ use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; -use bevy_openxr::xr_input::hand::{spawn_hand_entities, update_hand_states, HandStatesResource, update_emulated_hand_skeletons, draw_hand_entities}; +use bevy_openxr::xr_input::hand::OpenXrHandInput; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, @@ -23,13 +23,9 @@ fn main() { .add_plugins(FrameTimeDiagnosticsPlugin) .add_systems(Startup, setup) .add_systems(Update, proto_locomotion) - .add_systems(Startup, spawn_controllers_example) - .add_systems(Update, update_emulated_hand_skeletons) - .add_systems(PreUpdate, update_hand_states) - .add_systems(PostUpdate, draw_hand_entities) - .add_systems(Startup, spawn_hand_entities) .insert_resource(PrototypeLocomotionConfig::default()) - .insert_resource(HandStatesResource::default()) + .add_systems(Startup, spawn_controllers_example) + .add_plugins(OpenXrHandInput) .run(); } diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs index f02b5c4..63e5ef8 100644 --- a/src/xr_input/hand.rs +++ b/src/xr_input/hand.rs @@ -1,11 +1,51 @@ use std::f32::consts::PI; -use bevy::prelude::{Entity, Resource, Component, default, SpatialBundle, Commands, Res, ResMut, info, Transform, Query, Quat, Vec3, GlobalTransform, With, Gizmos, Color}; +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::{resources::{XrFrameState, XrInstance, XrSession}, input::XrInput, xr_input::Vec3Conv}; +use crate::{ + input::XrInput, + resources::{XrFrameState, XrInstance, XrSession}, + xr_input::Vec3Conv, +}; -use super::{trackers::{OpenXRTracker, OpenXRRightController, OpenXRLeftController}, Hand, oculus_touch::OculusController, hand_poses::get_simulated_open_hand_transforms}; +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(PostUpdate, draw_hand_entities) + .add_systems(Startup, spawn_hand_entities) + .insert_resource(HandStatesResource::default()) + .insert_resource(HandInputSource::default()); + } +} + +#[derive(Resource)] +pub enum HandInputSource { + Emulated, + OpenXr, +} + +impl Default for HandInputSource { + fn default() -> Self { + HandInputSource::Emulated + } +} #[derive(Resource, Default)] pub struct HandsResource { @@ -25,7 +65,15 @@ pub struct HandResource { 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() } + Self { + palm: Entity::PLACEHOLDER, + wrist: Entity::PLACEHOLDER, + thumb: Default::default(), + index: Default::default(), + middle: Default::default(), + ring: Default::default(), + little: Default::default(), + } } } @@ -38,7 +86,12 @@ pub struct ThumbResource { impl Default for ThumbResource { fn default() -> Self { - Self { metacarpal: Entity::PLACEHOLDER, proximal: Entity::PLACEHOLDER, distal: Entity::PLACEHOLDER, tip: Entity::PLACEHOLDER } + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } } } pub struct IndexResource { @@ -51,7 +104,13 @@ pub struct IndexResource { impl Default for IndexResource { fn default() -> Self { - Self { metacarpal: Entity::PLACEHOLDER, proximal: Entity::PLACEHOLDER, intermediate: Entity::PLACEHOLDER, distal: Entity::PLACEHOLDER, tip: Entity::PLACEHOLDER } + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + intermediate: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } } } pub struct MiddleResource { @@ -63,7 +122,13 @@ pub struct MiddleResource { } impl Default for MiddleResource { fn default() -> Self { - Self { metacarpal: Entity::PLACEHOLDER, proximal: Entity::PLACEHOLDER, intermediate: Entity::PLACEHOLDER, distal: Entity::PLACEHOLDER, tip: Entity::PLACEHOLDER } + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + intermediate: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } } } pub struct RingResource { @@ -75,7 +140,13 @@ pub struct RingResource { } impl Default for RingResource { fn default() -> Self { - Self { metacarpal: Entity::PLACEHOLDER, proximal: Entity::PLACEHOLDER, intermediate: Entity::PLACEHOLDER, distal: Entity::PLACEHOLDER, tip: Entity::PLACEHOLDER } + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + intermediate: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } } } pub struct LittleResource { @@ -87,7 +158,13 @@ pub struct LittleResource { } impl Default for LittleResource { fn default() -> Self { - Self { metacarpal: Entity::PLACEHOLDER, proximal: Entity::PLACEHOLDER, intermediate: Entity::PLACEHOLDER, distal: Entity::PLACEHOLDER, tip: Entity::PLACEHOLDER } + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + intermediate: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } } } @@ -176,7 +253,9 @@ pub fn spawn_hand_entities(mut commands: Commands) { 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::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, @@ -186,7 +265,9 @@ pub fn spawn_hand_entities(mut commands: Commands) { 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::LittleIntermediate => { + hand_resource.right.little.intermediate = boneid + } HandBone::LittleDistal => hand_resource.right.little.distal = boneid, HandBone::LittleTip => hand_resource.right.little.tip = boneid, }, @@ -915,39 +996,54 @@ fn log_hand(hand_pose: [Posef; 26]) { ); } - -pub fn update_emulated_hand_skeletons( +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 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, - ); + 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; } - None => info!("hand states resource not initialized yet"), } } @@ -987,4 +1083,4 @@ fn get_bone_gizmo_style(hand_bone: &HandBone) -> (f32, Color) { HandBone::LittleDistal => (0.004, Color::BLUE), HandBone::LittleTip => (0.002, Color::BLUE), } -} \ No newline at end of file +} From de75d7012b6b48d89fb9ac02e859b227f52c64ae Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sat, 14 Oct 2023 11:05:06 -0400 Subject: [PATCH 39/66] pulled debug rendering into its own plugin --- examples/xr.rs | 3 ++- src/xr_input/hand.rs | 11 ++++++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 363cbe9..f0d468d 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -5,7 +5,7 @@ use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; -use bevy_openxr::xr_input::hand::OpenXrHandInput; +use bevy_openxr::xr_input::hand::{OpenXrHandInput, HandInputDebugRenderer}; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, @@ -26,6 +26,7 @@ fn main() { .insert_resource(PrototypeLocomotionConfig::default()) .add_systems(Startup, spawn_controllers_example) .add_plugins(OpenXrHandInput) + .add_plugins(HandInputDebugRenderer) .run(); } diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs index 63e5ef8..f8a70ac 100644 --- a/src/xr_input/hand.rs +++ b/src/xr_input/hand.rs @@ -28,13 +28,22 @@ 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(PostUpdate, draw_hand_entities) .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, From c1819e975f4cae6798c9726b773be03754ee4708 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 26 Sep 2023 12:54:52 -0400 Subject: [PATCH 40/66] got a hand array --- examples/xr.rs | 105 ++++++++++++++++++++++++++++++++++++++++++- src/xr_input/hand.rs | 3 ++ 2 files changed, 107 insertions(+), 1 deletion(-) create mode 100644 src/xr_input/hand.rs diff --git a/examples/xr.rs b/examples/xr.rs index 548fe15..80eb3b7 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,12 +1,14 @@ use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; +use bevy_openxr::xr_input::{Vec3Conv, QuatConv}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, }; use bevy_openxr::DefaultXrPlugins; +use openxr::{Posef, Quaternionf, Vector3f, HandJoint}; fn main() { color_eyre::install().unwrap(); @@ -20,6 +22,7 @@ fn main() { .add_systems(Startup, setup) .add_systems(Update, proto_locomotion) .add_systems(Startup, spawn_controllers_example) + .add_systems(Update, spawn_skeleton) .insert_resource(PrototypeLocomotionConfig::default()) .run(); } @@ -62,11 +65,111 @@ 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() },)); } +fn spawn_skeleton(mut commands: Commands, mut gizmos: Gizmos) { + let hand_pose: [Posef; 26] = [ + Posef { position: Vector3f {x: -0.548, y: -0.161, z: -0.137}, orientation: Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}}, //palm + Posef { position: Vector3f {x: -0.548, y: 0.161, z: -0.137}, orientation: Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}}, + Posef { position: Vector3f {x: -0.529, y: -0.198, z: -0.126}, orientation: Quaternionf {x: -0.744, y: -0.530, z: 0.156,w: -0.376}}, + Posef { position: Vector3f {x: -0.533, y: -0.175, z: -0.090}, orientation: Quaternionf {x: -0.786, y: -0.550, z: 0.126,w: -0.254}}, + Posef { position: Vector3f {x: -0.544, y: -0.158, z: -0.069}, orientation: Quaternionf {x: -0.729, y: -0.564, z: 0.027,w: -0.387}}, + Posef { position: Vector3f {x: -0.557, y: -0.150, z: -0.065}, orientation: Quaternionf {x: -0.585, y: -0.548, z: -0.140,w: -0.582}}, + Posef { position: Vector3f {x: -0.521, y: -0.182, z: -0.136}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317,w: -0.376}}, + Posef { position: Vector3f {x: -0.550, y: -0.135, z: -0.102}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317,w: -0.376}}, + Posef { position: Vector3f {x: -0.571, y: -0.112, z: -0.082}, orientation: Quaternionf {x: -0.244, y: -0.843, z: 0.256,w: -0.404}}, + Posef { position: Vector3f {x: -0.585, y: -0.102, z: -0.070}, orientation: Quaternionf {x: -0.200, y: -0.866, z: 0.165,w: -0.428}}, + Posef { position: Vector3f {x: -0.593, y: -0.098, z: -0.064}, orientation: Quaternionf {x: -0.172, y: -0.874, z: 0.110,w: -0.440}}, + Posef { position: Vector3f {x: -0.527, y: -0.178, z: -0.144}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370,w: -0.401}}, + Posef { position: Vector3f {x: -0.559, y: -0.132, z: -0.119}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370,w: -0.401}}, + Posef { position: Vector3f {x: -0.582, y: -0.101, z: -0.104}, orientation: Quaternionf {x: -0.175, y: -0.809, z: 0.371,w: -0.420}}, + Posef { position: Vector3f {x: -0.599, y: -0.089, z: -0.092}, orientation: Quaternionf {x: -0.109, y: -0.856, z: 0.245,w: -0.443}}, + Posef { position: Vector3f {x: -0.608, y: -0.084, z: -0.086}, orientation: Quaternionf {x: -0.075, y: -0.871, z: 0.180,w: -0.450}}, + Posef { position: Vector3f {x: -0.535, y: -0.178, z: -0.152}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408,w: -0.445}}, + Posef { position: Vector3f {x: -0.568, y: -0.136, z: -0.137}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408,w: -0.445}}, + Posef { position: Vector3f {x: -0.590, y: -0.106, z: -0.130}, orientation: Quaternionf {x: -0.131, y: -0.762, z: 0.432,w: -0.464}}, + Posef { position: Vector3f {x: -0.607, y: -0.092, z: -0.122}, orientation: Quaternionf {x: -0.071, y: -0.810, z: 0.332,w: -0.477}}, + Posef { position: Vector3f {x: -0.617, y: -0.086, z: -0.117}, orientation: Quaternionf {x: -0.029, y: -0.836, z: 0.260,w: -0.482}}, + Posef { position: Vector3f {x: -0.544, y: -0.183, z: -0.159}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481,w: -0.452}}, + Posef { position: Vector3f {x: -0.576, y: -0.143, z: -0.152}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481,w: -0.452}}, + Posef { position: Vector3f {x: -0.594, y: -0.119, z: -0.154}, orientation: Quaternionf {x: -0.061, y: -0.684, z: 0.534,w: -0.493}}, + Posef { position: Vector3f {x: -0.607, y: -0.108, z: -0.152}, orientation: Quaternionf {x: 0.002, y: -0.745, z: 0.444,w: -0.498}}, + Posef { position: Vector3f {x: -0.616, y: -0.102, z: -0.150}, orientation: Quaternionf {x: 0.045, y: -0.780, z: 0.378,w: -0.496}}, + ]; + //cursed wrist math + let wrist_dist = Vec3{ x: 0.01, y: -0.05, z: 0.0 }; + let huh = Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}.to_quat(); + let why: Vec3 = huh.mul_vec3(wrist_dist); + + let offset = Vec3 { x: 0.548, y: 1.0, z: 0.137 }; + let palm = hand_pose[HandJoint::PALM]; + gizmos.sphere(palm.position.to_vec3() + offset, palm.orientation.to_quat(), 0.01, Color::WHITE); + let wrist = hand_pose[HandJoint::WRIST]; + gizmos.sphere(palm.position.to_vec3() + offset + why, palm.orientation.to_quat(), 0.01, Color::GRAY); + let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; + gizmos.sphere(thumb_meta.position.to_vec3() + offset, thumb_meta.orientation.to_quat(), 0.01, Color::RED); + let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; + gizmos.sphere(thumb_prox.position.to_vec3() + offset, thumb_prox.orientation.to_quat(), 0.008, Color::RED); + let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; + gizmos.sphere(thumb_dist.position.to_vec3() + offset, thumb_dist.orientation.to_quat(), 0.006, Color::RED); + let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; + gizmos.sphere(thumb_tip.position.to_vec3() + offset, thumb_tip.orientation.to_quat(), 0.004, Color::RED); + + let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; + gizmos.sphere(index_meta.position.to_vec3() + offset, index_meta.orientation.to_quat(), 0.01, Color::ORANGE); + let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; + gizmos.sphere(index_prox.position.to_vec3() + offset, index_prox.orientation.to_quat(), 0.008, Color::ORANGE); + let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; + gizmos.sphere(index_inter.position.to_vec3() + offset, index_inter.orientation.to_quat(), 0.006, Color::ORANGE); + let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; + gizmos.sphere(index_dist.position.to_vec3() + offset, index_dist.orientation.to_quat(), 0.004, Color::ORANGE); + let index_tip = hand_pose[HandJoint::INDEX_TIP]; + gizmos.sphere(index_tip.position.to_vec3() + offset, index_tip.orientation.to_quat(), 0.002, Color::ORANGE); + + let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; + gizmos.sphere(middle_meta.position.to_vec3() + offset, middle_meta.orientation.to_quat(), 0.01, Color::YELLOW); + let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; + gizmos.sphere(middle_prox.position.to_vec3() + offset, middle_prox.orientation.to_quat(), 0.008, Color::YELLOW); + let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; + gizmos.sphere(middle_inter.position.to_vec3() + offset, middle_inter.orientation.to_quat(), 0.006, Color::YELLOW); + let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; + gizmos.sphere(middle_dist.position.to_vec3() + offset, middle_dist.orientation.to_quat(), 0.004, Color::YELLOW); + let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; + gizmos.sphere(middle_tip.position.to_vec3() + offset, middle_tip.orientation.to_quat(), 0.002, Color::YELLOW); + + let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; + gizmos.sphere(ring_meta.position.to_vec3() + offset, ring_meta.orientation.to_quat(), 0.01, Color::GREEN); + let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; + gizmos.sphere(ring_prox.position.to_vec3() + offset, ring_prox.orientation.to_quat(), 0.008, Color::GREEN); + let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; + gizmos.sphere(ring_inter.position.to_vec3() + offset, ring_inter.orientation.to_quat(), 0.006, Color::GREEN); + let ring_dist = hand_pose[HandJoint::RING_DISTAL]; + gizmos.sphere(ring_dist.position.to_vec3() + offset, ring_dist.orientation.to_quat(), 0.004, Color::GREEN); + let ring_tip = hand_pose[HandJoint::RING_TIP]; + gizmos.sphere(ring_tip.position.to_vec3() + offset, ring_tip.orientation.to_quat(), 0.002, Color::GREEN); + + let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; + gizmos.sphere(little_meta.position.to_vec3() + offset, little_meta.orientation.to_quat(), 0.01, Color::BLUE); + let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; + gizmos.sphere(little_prox.position.to_vec3() + offset, little_prox.orientation.to_quat(), 0.008, Color::BLUE); + let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; + gizmos.sphere(little_inter.position.to_vec3() + offset, little_inter.orientation.to_quat(), 0.006, Color::BLUE); + let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; + gizmos.sphere(little_dist.position.to_vec3() + offset, little_dist.orientation.to_quat(), 0.004, Color::BLUE); + let little_tip = hand_pose[HandJoint::LITTLE_TIP]; + gizmos.sphere(little_tip.position.to_vec3() + offset, little_tip.orientation.to_quat(), 0.002, Color::BLUE); + + + + + + +} + + fn spawn_controllers_example(mut commands: Commands) { //left hand commands.spawn(( diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs new file mode 100644 index 0000000..0fcd267 --- /dev/null +++ b/src/xr_input/hand.rs @@ -0,0 +1,3 @@ +pub struct Hand { + pub HandJointPoses: [Posef; 26] +} \ No newline at end of file From a5fb724efe285aaf28eebf82cd4a5183773e8311 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 3 Oct 2023 14:28:33 -0400 Subject: [PATCH 41/66] well this is insane but it tracks the hand now --- examples/xr.rs | 158 ++++++++++++++++++++++++++++--------------------- 1 file changed, 92 insertions(+), 66 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 80eb3b7..1f3df09 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,3 +1,5 @@ +use std::f32::consts::PI; + use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; @@ -16,13 +18,13 @@ 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) - .add_systems(Update, spawn_skeleton) + .add_systems(Update, draw_skeleton_hand) .insert_resource(PrototypeLocomotionConfig::default()) .run(); } @@ -70,103 +72,127 @@ fn setup( },)); } -fn spawn_skeleton(mut commands: Commands, mut gizmos: Gizmos) { - let hand_pose: [Posef; 26] = [ - Posef { position: Vector3f {x: -0.548, y: -0.161, z: -0.137}, orientation: Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}}, //palm - Posef { position: Vector3f {x: -0.548, y: 0.161, z: -0.137}, orientation: Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}}, - Posef { position: Vector3f {x: -0.529, y: -0.198, z: -0.126}, orientation: Quaternionf {x: -0.744, y: -0.530, z: 0.156,w: -0.376}}, - Posef { position: Vector3f {x: -0.533, y: -0.175, z: -0.090}, orientation: Quaternionf {x: -0.786, y: -0.550, z: 0.126,w: -0.254}}, - Posef { position: Vector3f {x: -0.544, y: -0.158, z: -0.069}, orientation: Quaternionf {x: -0.729, y: -0.564, z: 0.027,w: -0.387}}, - Posef { position: Vector3f {x: -0.557, y: -0.150, z: -0.065}, orientation: Quaternionf {x: -0.585, y: -0.548, z: -0.140,w: -0.582}}, - Posef { position: Vector3f {x: -0.521, y: -0.182, z: -0.136}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317,w: -0.376}}, - Posef { position: Vector3f {x: -0.550, y: -0.135, z: -0.102}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317,w: -0.376}}, - Posef { position: Vector3f {x: -0.571, y: -0.112, z: -0.082}, orientation: Quaternionf {x: -0.244, y: -0.843, z: 0.256,w: -0.404}}, - Posef { position: Vector3f {x: -0.585, y: -0.102, z: -0.070}, orientation: Quaternionf {x: -0.200, y: -0.866, z: 0.165,w: -0.428}}, - Posef { position: Vector3f {x: -0.593, y: -0.098, z: -0.064}, orientation: Quaternionf {x: -0.172, y: -0.874, z: 0.110,w: -0.440}}, - Posef { position: Vector3f {x: -0.527, y: -0.178, z: -0.144}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370,w: -0.401}}, - Posef { position: Vector3f {x: -0.559, y: -0.132, z: -0.119}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370,w: -0.401}}, - Posef { position: Vector3f {x: -0.582, y: -0.101, z: -0.104}, orientation: Quaternionf {x: -0.175, y: -0.809, z: 0.371,w: -0.420}}, - Posef { position: Vector3f {x: -0.599, y: -0.089, z: -0.092}, orientation: Quaternionf {x: -0.109, y: -0.856, z: 0.245,w: -0.443}}, - Posef { position: Vector3f {x: -0.608, y: -0.084, z: -0.086}, orientation: Quaternionf {x: -0.075, y: -0.871, z: 0.180,w: -0.450}}, - Posef { position: Vector3f {x: -0.535, y: -0.178, z: -0.152}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408,w: -0.445}}, - Posef { position: Vector3f {x: -0.568, y: -0.136, z: -0.137}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408,w: -0.445}}, - Posef { position: Vector3f {x: -0.590, y: -0.106, z: -0.130}, orientation: Quaternionf {x: -0.131, y: -0.762, z: 0.432,w: -0.464}}, - Posef { position: Vector3f {x: -0.607, y: -0.092, z: -0.122}, orientation: Quaternionf {x: -0.071, y: -0.810, z: 0.332,w: -0.477}}, - Posef { position: Vector3f {x: -0.617, y: -0.086, z: -0.117}, orientation: Quaternionf {x: -0.029, y: -0.836, z: 0.260,w: -0.482}}, - Posef { position: Vector3f {x: -0.544, y: -0.183, z: -0.159}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481,w: -0.452}}, - Posef { position: Vector3f {x: -0.576, y: -0.143, z: -0.152}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481,w: -0.452}}, - Posef { position: Vector3f {x: -0.594, y: -0.119, z: -0.154}, orientation: Quaternionf {x: -0.061, y: -0.684, z: 0.534,w: -0.493}}, - Posef { position: Vector3f {x: -0.607, y: -0.108, z: -0.152}, orientation: Quaternionf {x: 0.002, y: -0.745, z: 0.444,w: -0.498}}, - Posef { position: Vector3f {x: -0.616, y: -0.102, z: -0.150}, orientation: Quaternionf {x: 0.045, y: -0.780, z: 0.378,w: -0.496}}, - ]; - //cursed wrist math - let wrist_dist = Vec3{ x: 0.01, y: -0.05, z: 0.0 }; - let huh = Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}.to_quat(); - let why: Vec3 = huh.mul_vec3(wrist_dist); +fn draw_skeleton_hand(mut commands: Commands, + mut gizmos: Gizmos, + right_controller_query: Query<( + &GlobalTransform, + With, +)>, ) { + //draw debug for controller grip center to match palm to + let right_transform = right_controller_query.get_single().unwrap().0; + let right_translation = right_transform.compute_transform().translation; + let right_quat = right_transform.compute_transform().rotation; + gizmos.sphere(right_translation, Quat::IDENTITY, 0.01, Color::PINK); + //we need to flip this i dont know why + let flip = Quat::from_rotation_x(PI); + let flap = Quat::from_rotation_z(-45.0*(PI/180.0)); + let controller_forward = right_quat.mul_quat(flip).mul_vec3(Vec3::Y); + let controller_backward = right_quat.mul_quat(flip).mul_quat(flap); + gizmos.ray(right_translation, controller_forward, Color::PINK); - let offset = Vec3 { x: 0.548, y: 1.0, z: 0.137 }; + let hand_pose: [Posef; 26] = [ + Posef { position: Vector3f {x: -0.548, y: -0.161, z: -0.137}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, //palm + Posef { position: Vector3f {x: -0.548, y: 0.161, z: -0.137}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, + Posef { position: Vector3f {x: -0.529, y: -0.198, z: -0.126}, orientation: Quaternionf {x: -0.744, y: -0.530, z: 0.156, w: -0.376}}, + Posef { position: Vector3f {x: -0.533, y: -0.175, z: -0.090}, orientation: Quaternionf {x: -0.786, y: -0.550, z: 0.126, w: -0.254}}, + Posef { position: Vector3f {x: -0.544, y: -0.158, z: -0.069}, orientation: Quaternionf {x: -0.729, y: -0.564, z: 0.027, w: -0.387}}, + Posef { position: Vector3f {x: -0.557, y: -0.150, z: -0.065}, orientation: Quaternionf {x: -0.585, y: -0.548, z: -0.140,w: -0.582}}, + Posef { position: Vector3f {x: -0.521, y: -0.182, z: -0.136}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317, w: -0.376}}, + Posef { position: Vector3f {x: -0.550, y: -0.135, z: -0.102}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317, w: -0.376}}, + Posef { position: Vector3f {x: -0.571, y: -0.112, z: -0.082}, orientation: Quaternionf {x: -0.244, y: -0.843, z: 0.256, w: -0.404}}, + Posef { position: Vector3f {x: -0.585, y: -0.102, z: -0.070}, orientation: Quaternionf {x: -0.200, y: -0.866, z: 0.165, w: -0.428}}, + Posef { position: Vector3f {x: -0.593, y: -0.098, z: -0.064}, orientation: Quaternionf {x: -0.172, y: -0.874, z: 0.110, w: -0.440}}, + Posef { position: Vector3f {x: -0.527, y: -0.178, z: -0.144}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370, w: -0.401}}, + Posef { position: Vector3f {x: -0.559, y: -0.132, z: -0.119}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370, w: -0.401}}, + Posef { position: Vector3f {x: -0.582, y: -0.101, z: -0.104}, orientation: Quaternionf {x: -0.175, y: -0.809, z: 0.371, w: -0.420}}, + Posef { position: Vector3f {x: -0.599, y: -0.089, z: -0.092}, orientation: Quaternionf {x: -0.109, y: -0.856, z: 0.245, w: -0.443}}, + Posef { position: Vector3f {x: -0.608, y: -0.084, z: -0.086}, orientation: Quaternionf {x: -0.075, y: -0.871, z: 0.180, w: -0.450}}, + Posef { position: Vector3f {x: -0.535, y: -0.178, z: -0.152}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408, w: -0.445}}, + Posef { position: Vector3f {x: -0.568, y: -0.136, z: -0.137}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408, w: -0.445}}, + Posef { position: Vector3f {x: -0.590, y: -0.106, z: -0.130}, orientation: Quaternionf {x: -0.131, y: -0.762, z: 0.432, w: -0.464}}, + Posef { position: Vector3f {x: -0.607, y: -0.092, z: -0.122}, orientation: Quaternionf {x: -0.071, y: -0.810, z: 0.332, w: -0.477}}, + Posef { position: Vector3f {x: -0.617, y: -0.086, z: -0.117}, orientation: Quaternionf {x: -0.029, y: -0.836, z: 0.260, w: -0.482}}, + Posef { position: Vector3f {x: -0.544, y: -0.183, z: -0.159}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481, w: -0.452}}, + Posef { position: Vector3f {x: -0.576, y: -0.143, z: -0.152}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481, w: -0.452}}, + Posef { position: Vector3f {x: -0.594, y: -0.119, z: -0.154}, orientation: Quaternionf {x: -0.061, y: -0.684, z: 0.534, w: -0.493}}, + Posef { position: Vector3f {x: -0.607, y: -0.108, z: -0.152}, orientation: Quaternionf {x: 0.002, y: -0.745, z: 0.444, w: -0.498}}, + Posef { position: Vector3f {x: -0.616, y: -0.102, z: -0.150}, orientation: Quaternionf {x: 0.045, y: -0.780, z: 0.378, w: -0.496}}, + ]; + + //cursed wrist math + let wrist_dist = Vec3{ x: -0.01, y: -0.040, z: -0.015}; + + //cursed offset + let palm_negation = Vec3 { x: 0.548, y: 0.161, z: 0.137 }; + let offset = right_translation; + + //old stuff dont touch for now let palm = hand_pose[HandJoint::PALM]; - gizmos.sphere(palm.position.to_vec3() + offset, palm.orientation.to_quat(), 0.01, Color::WHITE); - let wrist = hand_pose[HandJoint::WRIST]; - gizmos.sphere(palm.position.to_vec3() + offset + why, palm.orientation.to_quat(), 0.01, Color::GRAY); + gizmos.sphere(palm.position.to_vec3() + offset + palm_negation, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); + + let rotated_wfp = palm.position.to_vec3() + right_quat.mul_quat(flip).mul_vec3(wrist_dist); + gizmos.sphere(offset + palm_negation + rotated_wfp, palm.orientation.to_quat(), 0.01, Color::GRAY); + + let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; - gizmos.sphere(thumb_meta.position.to_vec3() + offset, thumb_meta.orientation.to_quat(), 0.01, Color::RED); + draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, palm_negation, offset); + let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; - gizmos.sphere(thumb_prox.position.to_vec3() + offset, thumb_prox.orientation.to_quat(), 0.008, Color::RED); + draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, palm_negation, offset); let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; - gizmos.sphere(thumb_dist.position.to_vec3() + offset, thumb_dist.orientation.to_quat(), 0.006, Color::RED); + draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, palm_negation, offset); let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; - gizmos.sphere(thumb_tip.position.to_vec3() + offset, thumb_tip.orientation.to_quat(), 0.004, Color::RED); + draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, palm_negation, offset); let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; - gizmos.sphere(index_meta.position.to_vec3() + offset, index_meta.orientation.to_quat(), 0.01, Color::ORANGE); + draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, palm_negation, offset); let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; - gizmos.sphere(index_prox.position.to_vec3() + offset, index_prox.orientation.to_quat(), 0.008, Color::ORANGE); + draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, palm_negation, offset); let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; - gizmos.sphere(index_inter.position.to_vec3() + offset, index_inter.orientation.to_quat(), 0.006, Color::ORANGE); + draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, palm_negation, offset); let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; - gizmos.sphere(index_dist.position.to_vec3() + offset, index_dist.orientation.to_quat(), 0.004, Color::ORANGE); + draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, palm_negation, offset); let index_tip = hand_pose[HandJoint::INDEX_TIP]; - gizmos.sphere(index_tip.position.to_vec3() + offset, index_tip.orientation.to_quat(), 0.002, Color::ORANGE); + draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, palm_negation, offset); let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; - gizmos.sphere(middle_meta.position.to_vec3() + offset, middle_meta.orientation.to_quat(), 0.01, Color::YELLOW); + draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, palm_negation, offset); let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; - gizmos.sphere(middle_prox.position.to_vec3() + offset, middle_prox.orientation.to_quat(), 0.008, Color::YELLOW); + draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, palm_negation, offset); let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; - gizmos.sphere(middle_inter.position.to_vec3() + offset, middle_inter.orientation.to_quat(), 0.006, Color::YELLOW); + draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, palm_negation, offset); let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; - gizmos.sphere(middle_dist.position.to_vec3() + offset, middle_dist.orientation.to_quat(), 0.004, Color::YELLOW); + draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, palm_negation, offset); let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; - gizmos.sphere(middle_tip.position.to_vec3() + offset, middle_tip.orientation.to_quat(), 0.002, Color::YELLOW); + draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, palm_negation, offset); let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; - gizmos.sphere(ring_meta.position.to_vec3() + offset, ring_meta.orientation.to_quat(), 0.01, Color::GREEN); + draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, palm_negation, offset); let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; - gizmos.sphere(ring_prox.position.to_vec3() + offset, ring_prox.orientation.to_quat(), 0.008, Color::GREEN); + draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, palm_negation, offset); let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; - gizmos.sphere(ring_inter.position.to_vec3() + offset, ring_inter.orientation.to_quat(), 0.006, Color::GREEN); + draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, palm_negation, offset); let ring_dist = hand_pose[HandJoint::RING_DISTAL]; - gizmos.sphere(ring_dist.position.to_vec3() + offset, ring_dist.orientation.to_quat(), 0.004, Color::GREEN); + draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, palm_negation, offset); let ring_tip = hand_pose[HandJoint::RING_TIP]; - gizmos.sphere(ring_tip.position.to_vec3() + offset, ring_tip.orientation.to_quat(), 0.002, Color::GREEN); + draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, palm_negation, offset); let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; - gizmos.sphere(little_meta.position.to_vec3() + offset, little_meta.orientation.to_quat(), 0.01, Color::BLUE); + draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, palm_negation, offset); let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; - gizmos.sphere(little_prox.position.to_vec3() + offset, little_prox.orientation.to_quat(), 0.008, Color::BLUE); + draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, palm_negation, offset); let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; - gizmos.sphere(little_inter.position.to_vec3() + offset, little_inter.orientation.to_quat(), 0.006, Color::BLUE); + draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, palm_negation, offset); let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; - gizmos.sphere(little_dist.position.to_vec3() + offset, little_dist.orientation.to_quat(), 0.004, Color::BLUE); + draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, palm_negation, offset); let little_tip = hand_pose[HandJoint::LITTLE_TIP]; - gizmos.sphere(little_tip.position.to_vec3() + offset, little_tip.orientation.to_quat(), 0.002, Color::BLUE); - - - + draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, palm_negation, offset); +} +fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, palm_negation: Vec3, offset: Vec3) { + gizmos.sphere(controller_backwards.mul_vec3(joint_pos + palm_negation) + offset, joint_rot, radius, color); } From 4ab217232458cdfa1cd9c5999965989bf8a609b0 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 3 Oct 2023 15:03:34 -0400 Subject: [PATCH 42/66] normalized hand poses to palm --- examples/xr.rs | 157 +++++++++++++++++++++++++++++++------------------ 1 file changed, 99 insertions(+), 58 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 1f3df09..b1305da 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -82,117 +82,158 @@ fn draw_skeleton_hand(mut commands: Commands, let right_transform = right_controller_query.get_single().unwrap().0; let right_translation = right_transform.compute_transform().translation; let right_quat = right_transform.compute_transform().rotation; - gizmos.sphere(right_translation, Quat::IDENTITY, 0.01, Color::PINK); //we need to flip this i dont know why let flip = Quat::from_rotation_x(PI); let flap = Quat::from_rotation_z(-45.0*(PI/180.0)); - let controller_forward = right_quat.mul_quat(flip).mul_vec3(Vec3::Y); let controller_backward = right_quat.mul_quat(flip).mul_quat(flap); - gizmos.ray(right_translation, controller_forward, Color::PINK); let hand_pose: [Posef; 26] = [ - Posef { position: Vector3f {x: -0.548, y: -0.161, z: -0.137}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, //palm - Posef { position: Vector3f {x: -0.548, y: 0.161, z: -0.137}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, - Posef { position: Vector3f {x: -0.529, y: -0.198, z: -0.126}, orientation: Quaternionf {x: -0.744, y: -0.530, z: 0.156, w: -0.376}}, - Posef { position: Vector3f {x: -0.533, y: -0.175, z: -0.090}, orientation: Quaternionf {x: -0.786, y: -0.550, z: 0.126, w: -0.254}}, - Posef { position: Vector3f {x: -0.544, y: -0.158, z: -0.069}, orientation: Quaternionf {x: -0.729, y: -0.564, z: 0.027, w: -0.387}}, - Posef { position: Vector3f {x: -0.557, y: -0.150, z: -0.065}, orientation: Quaternionf {x: -0.585, y: -0.548, z: -0.140,w: -0.582}}, - Posef { position: Vector3f {x: -0.521, y: -0.182, z: -0.136}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317, w: -0.376}}, - Posef { position: Vector3f {x: -0.550, y: -0.135, z: -0.102}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317, w: -0.376}}, - Posef { position: Vector3f {x: -0.571, y: -0.112, z: -0.082}, orientation: Quaternionf {x: -0.244, y: -0.843, z: 0.256, w: -0.404}}, - Posef { position: Vector3f {x: -0.585, y: -0.102, z: -0.070}, orientation: Quaternionf {x: -0.200, y: -0.866, z: 0.165, w: -0.428}}, - Posef { position: Vector3f {x: -0.593, y: -0.098, z: -0.064}, orientation: Quaternionf {x: -0.172, y: -0.874, z: 0.110, w: -0.440}}, - Posef { position: Vector3f {x: -0.527, y: -0.178, z: -0.144}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370, w: -0.401}}, - Posef { position: Vector3f {x: -0.559, y: -0.132, z: -0.119}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370, w: -0.401}}, - Posef { position: Vector3f {x: -0.582, y: -0.101, z: -0.104}, orientation: Quaternionf {x: -0.175, y: -0.809, z: 0.371, w: -0.420}}, - Posef { position: Vector3f {x: -0.599, y: -0.089, z: -0.092}, orientation: Quaternionf {x: -0.109, y: -0.856, z: 0.245, w: -0.443}}, - Posef { position: Vector3f {x: -0.608, y: -0.084, z: -0.086}, orientation: Quaternionf {x: -0.075, y: -0.871, z: 0.180, w: -0.450}}, - Posef { position: Vector3f {x: -0.535, y: -0.178, z: -0.152}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408, w: -0.445}}, - Posef { position: Vector3f {x: -0.568, y: -0.136, z: -0.137}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408, w: -0.445}}, - Posef { position: Vector3f {x: -0.590, y: -0.106, z: -0.130}, orientation: Quaternionf {x: -0.131, y: -0.762, z: 0.432, w: -0.464}}, - Posef { position: Vector3f {x: -0.607, y: -0.092, z: -0.122}, orientation: Quaternionf {x: -0.071, y: -0.810, z: 0.332, w: -0.477}}, - Posef { position: Vector3f {x: -0.617, y: -0.086, z: -0.117}, orientation: Quaternionf {x: -0.029, y: -0.836, z: 0.260, w: -0.482}}, - Posef { position: Vector3f {x: -0.544, y: -0.183, z: -0.159}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481, w: -0.452}}, - Posef { position: Vector3f {x: -0.576, y: -0.143, z: -0.152}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481, w: -0.452}}, - Posef { position: Vector3f {x: -0.594, y: -0.119, z: -0.154}, orientation: Quaternionf {x: -0.061, y: -0.684, z: 0.534, w: -0.493}}, - Posef { position: Vector3f {x: -0.607, y: -0.108, z: -0.152}, orientation: Quaternionf {x: 0.002, y: -0.745, z: 0.444, w: -0.498}}, - Posef { position: Vector3f {x: -0.616, y: -0.102, z: -0.150}, orientation: Quaternionf {x: 0.045, y: -0.780, z: 0.378, w: -0.496}}, + 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.0, y: 0.32, z: 0.0}, 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}}, ]; + //log_hand(hand_pose); + + //cursed wrist math let wrist_dist = Vec3{ x: -0.01, y: -0.040, z: -0.015}; //cursed offset - let palm_negation = Vec3 { x: 0.548, y: 0.161, z: 0.137 }; let offset = right_translation; //old stuff dont touch for now let palm = hand_pose[HandJoint::PALM]; - gizmos.sphere(palm.position.to_vec3() + offset + palm_negation, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); + gizmos.sphere(palm.position.to_vec3() + offset, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); let rotated_wfp = palm.position.to_vec3() + right_quat.mul_quat(flip).mul_vec3(wrist_dist); - gizmos.sphere(offset + palm_negation + rotated_wfp, palm.orientation.to_quat(), 0.01, Color::GRAY); + gizmos.sphere(offset + rotated_wfp, palm.orientation.to_quat(), 0.01, Color::GRAY); let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; - draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, offset); let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; - draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, offset); let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; - draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, offset); let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; - draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, offset); let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; - draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, offset); let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; - draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, offset); let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; - draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, offset); let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; - draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, offset); let index_tip = hand_pose[HandJoint::INDEX_TIP]; - draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, offset); let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; - draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, offset); let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; - draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, offset); let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; - draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, offset); let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; - draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, offset); let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; - draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, offset); let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; - draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, offset); let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; - draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, offset); let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; - draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, offset); let ring_dist = hand_pose[HandJoint::RING_DISTAL]; - draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, offset); let ring_tip = hand_pose[HandJoint::RING_TIP]; - draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, offset); let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; - draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, offset); let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; - draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, offset); let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; - draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, offset); let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; - draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, offset); let little_tip = hand_pose[HandJoint::LITTLE_TIP]; - draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, offset); + + } -fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, palm_negation: Vec3, offset: Vec3) { - gizmos.sphere(controller_backwards.mul_vec3(joint_pos + palm_negation) + offset, joint_rot, radius, color); +fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, offset: Vec3) { + gizmos.sphere(controller_backwards.mul_vec3(joint_pos) + offset, joint_rot, radius, color); +} + +fn log_hand(hand_pose: [Posef; 26]) { + let palm_vec = hand_pose[HandJoint::PALM].position.to_vec3(); + info!("palm: {}", hand_pose[HandJoint::PALM].position.to_vec3() - palm_vec); + info!("wrist: {}", hand_pose[HandJoint::WRIST].position.to_vec3() - palm_vec); + + info!("tm: {}", hand_pose[HandJoint::THUMB_METACARPAL].position.to_vec3() - palm_vec); + info!("tp: {}", hand_pose[HandJoint::THUMB_PROXIMAL].position.to_vec3() - palm_vec); + info!("td: {}", hand_pose[HandJoint::THUMB_DISTAL].position.to_vec3() - palm_vec); + info!("tt: {}", hand_pose[HandJoint::THUMB_TIP].position.to_vec3() - palm_vec); + + info!("im: {}", hand_pose[HandJoint::INDEX_METACARPAL].position.to_vec3() - palm_vec); + info!("ip: {}", hand_pose[HandJoint::INDEX_PROXIMAL].position.to_vec3() - palm_vec); + info!("ii: {}", hand_pose[HandJoint::INDEX_INTERMEDIATE].position.to_vec3() - palm_vec); + info!("id: {}", hand_pose[HandJoint::INDEX_DISTAL].position.to_vec3() - palm_vec); + info!("it: {}", hand_pose[HandJoint::INDEX_TIP].position.to_vec3() - palm_vec); + + info!("mm: {}", hand_pose[HandJoint::MIDDLE_METACARPAL].position.to_vec3() - palm_vec); + info!("mp: {}", hand_pose[HandJoint::MIDDLE_PROXIMAL].position.to_vec3() - palm_vec); + info!("mi: {}", hand_pose[HandJoint::MIDDLE_INTERMEDIATE].position.to_vec3() - palm_vec); + info!("md: {}", hand_pose[HandJoint::MIDDLE_DISTAL].position.to_vec3() - palm_vec); + info!("mt: {}", hand_pose[HandJoint::MIDDLE_TIP].position.to_vec3() - palm_vec); + + info!("rm: {}", hand_pose[HandJoint::RING_METACARPAL].position.to_vec3() - palm_vec); + info!("rp: {}", hand_pose[HandJoint::RING_PROXIMAL].position.to_vec3() - palm_vec); + info!("ri: {}", hand_pose[HandJoint::RING_INTERMEDIATE].position.to_vec3() - palm_vec); + info!("rd: {}", hand_pose[HandJoint::RING_DISTAL].position.to_vec3() - palm_vec); + info!("rt: {}", hand_pose[HandJoint::RING_TIP].position.to_vec3() - palm_vec); + + info!("lm: {}", hand_pose[HandJoint::LITTLE_METACARPAL].position.to_vec3() - palm_vec); + info!("lp: {}", hand_pose[HandJoint::LITTLE_PROXIMAL].position.to_vec3() - palm_vec); + info!("li: {}", hand_pose[HandJoint::LITTLE_INTERMEDIATE].position.to_vec3() - palm_vec); + info!("ld: {}", hand_pose[HandJoint::LITTLE_DISTAL].position.to_vec3() - palm_vec); + info!("lt: {}", hand_pose[HandJoint::LITTLE_TIP].position.to_vec3() - palm_vec); } From 023e4036b2ded5012bfd6e5cee9ee513a412f0c7 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 3 Oct 2023 15:47:18 -0400 Subject: [PATCH 43/66] both hands work --- examples/xr.rs | 117 ++++++++++++++++++++++++++++--------------------- 1 file changed, 68 insertions(+), 49 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index b1305da..3d8058b 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,9 +1,10 @@ use std::f32::consts::PI; +use std::ops::Mul; use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; -use bevy_openxr::xr_input::{Vec3Conv, QuatConv}; +use bevy_openxr::xr_input::{Vec3Conv, QuatConv, Hand}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ @@ -24,7 +25,7 @@ fn main() { .add_systems(Startup, setup) .add_systems(Update, proto_locomotion) .add_systems(Startup, spawn_controllers_example) - .add_systems(Update, draw_skeleton_hand) + .add_systems(Update, draw_skeleton_hands) .insert_resource(PrototypeLocomotionConfig::default()) .run(); } @@ -72,24 +73,39 @@ fn setup( },)); } -fn draw_skeleton_hand(mut commands: Commands, +fn draw_skeleton_hands(mut commands: Commands, mut gizmos: Gizmos, right_controller_query: Query<( &GlobalTransform, With, -)>, ) { +)>, +left_controller_query: Query<( + &GlobalTransform, + With, +)>,) { + + let left_hand_transform = left_controller_query.get_single().unwrap().0.compute_transform(); + draw_hand(&mut gizmos, left_hand_transform, Hand::Left); + let right_hand_transform = right_controller_query.get_single().unwrap().0.compute_transform(); + draw_hand(&mut gizmos, right_hand_transform, Hand::Right) + + + +} + +fn draw_hand(mut gizmos: &mut Gizmos, + controller_transform: Transform, +hand: Hand) { //draw debug for controller grip center to match palm to - let right_transform = right_controller_query.get_single().unwrap().0; - let right_translation = right_transform.compute_transform().translation; - let right_quat = right_transform.compute_transform().rotation; + let hand_translation = controller_transform.translation; + let hand_quat = controller_transform.rotation; //we need to flip this i dont know why let flip = Quat::from_rotation_x(PI); - let flap = Quat::from_rotation_z(-45.0*(PI/180.0)); - let controller_backward = right_quat.mul_quat(flip).mul_quat(flap); + let controller_backward = hand_quat.mul_quat(flip); - let hand_pose: [Posef; 26] = [ + let test: [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.0, y: 0.32, z: 0.0}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, + 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}}, @@ -121,80 +137,83 @@ fn draw_skeleton_hand(mut commands: Commands, 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}}, ]; + let hand_pose = flip_hand_pose(test.clone(), hand); + //log_hand(hand_pose); - - //cursed wrist math - let wrist_dist = Vec3{ x: -0.01, y: -0.040, z: -0.015}; - - //cursed offset - let offset = right_translation; - - //old stuff dont touch for now let palm = hand_pose[HandJoint::PALM]; - gizmos.sphere(palm.position.to_vec3() + offset, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); + gizmos.sphere(palm.position.to_vec3() + hand_translation, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); - let rotated_wfp = palm.position.to_vec3() + right_quat.mul_quat(flip).mul_vec3(wrist_dist); - gizmos.sphere(offset + rotated_wfp, palm.orientation.to_quat(), 0.01, Color::GRAY); + let wrist = hand_pose[HandJoint::WRIST]; + draw_joint(&mut gizmos, wrist.position.to_vec3(), wrist.orientation.to_quat(), 0.01, Color::GRAY, controller_backward, hand_translation); let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; - draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, offset); + draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, hand_translation); let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; - draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, offset); + draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, hand_translation); let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; - draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, offset); + draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, hand_translation); let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; - draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, offset); + draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, hand_translation); let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; - draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, hand_translation); let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; - draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, hand_translation); let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; - draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, hand_translation); let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; - draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, hand_translation); let index_tip = hand_pose[HandJoint::INDEX_TIP]; - draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, hand_translation); let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; - draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, hand_translation); let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; - draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, hand_translation); let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; - draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, hand_translation); let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; - draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, hand_translation); let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; - draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, hand_translation); let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; - draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, hand_translation); let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; - draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, hand_translation); let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; - draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, hand_translation); let ring_dist = hand_pose[HandJoint::RING_DISTAL]; - draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, hand_translation); let ring_tip = hand_pose[HandJoint::RING_TIP]; - draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, hand_translation); let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; - draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, offset); + draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, hand_translation); let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; - draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, offset); + draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, hand_translation); let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; - draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, offset); + draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, hand_translation); let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; - draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, offset); + draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, hand_translation); let little_tip = hand_pose[HandJoint::LITTLE_TIP]; - draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, offset); - - - + draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, hand_translation); +} +fn flip_hand_pose(hand_pose: [Posef; 26], hand: Hand) -> [Posef; 26] { + let mut new_pose = hand_pose; + match hand { + Hand::Left => { + for pose in new_pose.iter_mut() { + pose.position.x = -pose.position.x; + } + }, + Hand::Right => (), + } + return new_pose; } fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, offset: Vec3) { From 0f2c063a44e05965d5900ba5b215a59f1b779bb1 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sat, 7 Oct 2023 17:05:47 -0400 Subject: [PATCH 44/66] left hand works too --- examples/xr.rs | 844 ++++++++++++++++++++++++++++++------- src/xr_input/hand_poses.rs | 520 +++++++++++++++++++++++ src/xr_input/mod.rs | 1 + 3 files changed, 1224 insertions(+), 141 deletions(-) create mode 100644 src/xr_input/hand_poses.rs diff --git a/examples/xr.rs b/examples/xr.rs index 3d8058b..d34c4b7 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -2,16 +2,18 @@ use std::f32::consts::PI; use std::ops::Mul; use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; -use bevy::prelude::*; +use bevy::math::vec3; use bevy::transform::components::Transform; -use bevy_openxr::xr_input::{Vec3Conv, QuatConv, Hand}; +use bevy::{gizmos, prelude::*}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; +use bevy_openxr::xr_input::hand_poses::*; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, }; +use bevy_openxr::xr_input::{Hand, QuatConv, Vec3Conv}; use bevy_openxr::DefaultXrPlugins; -use openxr::{Posef, Quaternionf, Vector3f, HandJoint}; +use openxr::{HandJoint, Posef, Quaternionf, Vector3f}; fn main() { color_eyre::install().unwrap(); @@ -68,34 +70,306 @@ fn setup( }); // camera commands.spawn((Camera3dBundle { - transform: Transform::from_xyz(0.25, 1.25, 0.0).looking_at(Vec3{x: -0.548, y: -0.161, z: -0.137}, 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() },)); } -fn draw_skeleton_hands(mut commands: Commands, +fn draw_skeleton_hands( + mut commands: Commands, mut gizmos: Gizmos, - right_controller_query: Query<( - &GlobalTransform, - With, -)>, -left_controller_query: Query<( - &GlobalTransform, - With, -)>,) { - - let left_hand_transform = left_controller_query.get_single().unwrap().0.compute_transform(); - draw_hand(&mut gizmos, left_hand_transform, Hand::Left); - let right_hand_transform = right_controller_query.get_single().unwrap().0.compute_transform(); - draw_hand(&mut gizmos, right_hand_transform, Hand::Right) - - - + right_controller_query: Query<(&GlobalTransform, With)>, + left_controller_query: Query<(&GlobalTransform, With)>, +) { + let left_hand_transform = left_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left); + let right_hand_transform = right_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); + draw_hand_bones(&mut gizmos, right_hand_transform, Hand::Right); } -fn draw_hand(mut gizmos: &mut Gizmos, - controller_transform: Transform, -hand: Hand) { +fn pose_array_to_transform_array(hand_pose: [Posef; 26]) -> [Transform; 26] { + let mut result_array: [Transform; 26] = [Transform::default(); 26]; + for (place, data) in result_array.iter_mut().zip(hand_pose.iter()) { + *place = Transform { + translation: data.position.to_vec3(), + rotation: data.orientation.to_quat(), + scale: Vec3::splat(1.0), + } + } + return result_array; +} + +fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: 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, + }; + //get paml 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); + //draw debug rays + gizmos.ray( + hand_translation, + palm_quat.mul_vec3(Vec3::Z * 0.2), + Color::BLUE, + ); + gizmos.ray( + hand_translation, + palm_quat.mul_vec3(Vec3::Y * 0.2), + Color::GREEN, + ); + gizmos.ray( + hand_translation, + palm_quat.mul_vec3(Vec3::X * 0.2), + Color::RED, + ); + //get simulated bones + let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); + //draw controller-palm bone(should be zero length) + let palm = hand_transform_array[HandJoint::PALM]; + gizmos.ray(hand_translation, palm.translation, Color::WHITE); + //draw palm-wrist + let wrist = hand_transform_array[HandJoint::WRIST]; + gizmos.ray( + hand_translation + palm.translation, + palm_quat.mul_vec3(wrist.translation), + Color::GRAY, + ); + + //thumb + //better finger drawing? + 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 color = Color::RED; + let splay = Quat::from_rotation_y(splay_direction * 30.0 * PI / 180.0); + let splay_quat = palm_quat.mul_quat(splay); + for bone in thumb_joints.iter() { + match prior_start { + Some(start) => { + let tp_lrot = Quat::from_rotation_y(splay_direction * 5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } + + //better finger drawing? + 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 color = Color::ORANGE; + 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 tp_lrot = Quat::from_rotation_x(-5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } + + //better finger drawing? + 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 color = Color::YELLOW; + 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 tp_lrot = Quat::from_rotation_x(-5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } + //better finger drawing? + 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 color = Color::GREEN; + 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 tp_lrot = Quat::from_rotation_x(-5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } + + //better finger drawing? + 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 color = Color::BLUE; + 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 tp_lrot = Quat::from_rotation_x(-5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } +} + +fn draw_hand(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: Hand) { //draw debug for controller grip center to match palm to let hand_translation = controller_transform.translation; let hand_quat = controller_transform.rotation; @@ -103,104 +377,280 @@ hand: Hand) { let flip = Quat::from_rotation_x(PI); let controller_backward = hand_quat.mul_quat(flip); - let test: [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}}, + let test_hand_pose = get_test_hand_pose_array(); - 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}}, + let hand_pose = flip_hand_pose(test_hand_pose.clone(), hand); - 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}}, + // let hand_transform_array: [Transform; 26] = pose_array_to_transform_array(hand_pose); + let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); - 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}}, + let palm = hand_transform_array[HandJoint::PALM]; + gizmos.sphere( + palm.translation + hand_translation, + palm.rotation.mul_quat(controller_backward), + 0.01, + Color::WHITE, + ); - 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}}, + let wrist = hand_transform_array[HandJoint::WRIST]; + draw_joint( + &mut gizmos, + wrist.translation, + wrist.rotation, + 0.01, + Color::GRAY, + controller_backward, + hand_translation, + ); - 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}}, - ]; + let thumb_meta = hand_transform_array[HandJoint::THUMB_METACARPAL]; + draw_joint( + &mut gizmos, + thumb_meta.translation, + thumb_meta.rotation, + 0.01, + Color::RED, + controller_backward, + hand_translation, + ); - let hand_pose = flip_hand_pose(test.clone(), hand); + let thumb_prox = hand_transform_array[HandJoint::THUMB_PROXIMAL]; + draw_joint( + &mut gizmos, + thumb_prox.translation, + thumb_prox.rotation, + 0.008, + Color::RED, + controller_backward, + hand_translation, + ); + let thumb_dist = hand_transform_array[HandJoint::THUMB_DISTAL]; + draw_joint( + &mut gizmos, + thumb_dist.translation, + thumb_dist.rotation, + 0.006, + Color::RED, + controller_backward, + hand_translation, + ); + let thumb_tip = hand_transform_array[HandJoint::THUMB_TIP]; + draw_joint( + &mut gizmos, + thumb_tip.translation, + thumb_tip.rotation, + 0.004, + Color::RED, + controller_backward, + hand_translation, + ); - //log_hand(hand_pose); + let index_meta = hand_transform_array[HandJoint::INDEX_METACARPAL]; + draw_joint( + &mut gizmos, + index_meta.translation, + index_meta.rotation, + 0.01, + Color::ORANGE, + controller_backward, + hand_translation, + ); + let index_prox = hand_transform_array[HandJoint::INDEX_PROXIMAL]; + draw_joint( + &mut gizmos, + index_prox.translation, + index_prox.rotation, + 0.008, + Color::ORANGE, + controller_backward, + hand_translation, + ); - let palm = hand_pose[HandJoint::PALM]; - gizmos.sphere(palm.position.to_vec3() + hand_translation, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); + let index_inter = hand_transform_array[HandJoint::INDEX_INTERMEDIATE]; + draw_joint( + &mut gizmos, + index_inter.translation, + index_inter.rotation, + 0.006, + Color::ORANGE, + controller_backward, + hand_translation, + ); - let wrist = hand_pose[HandJoint::WRIST]; - draw_joint(&mut gizmos, wrist.position.to_vec3(), wrist.orientation.to_quat(), 0.01, Color::GRAY, controller_backward, hand_translation); + let index_dist = hand_transform_array[HandJoint::INDEX_DISTAL]; + draw_joint( + &mut gizmos, + index_dist.translation, + index_dist.rotation, + 0.004, + Color::ORANGE, + controller_backward, + hand_translation, + ); + let index_tip = hand_transform_array[HandJoint::INDEX_TIP]; + draw_joint( + &mut gizmos, + index_tip.translation, + index_tip.rotation, + 0.002, + Color::ORANGE, + controller_backward, + hand_translation, + ); - let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; - draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, hand_translation); + let middle_meta = hand_transform_array[HandJoint::MIDDLE_METACARPAL]; + draw_joint( + &mut gizmos, + middle_meta.translation, + middle_meta.rotation, + 0.01, + Color::YELLOW, + controller_backward, + hand_translation, + ); + let middle_prox = hand_transform_array[HandJoint::MIDDLE_PROXIMAL]; + draw_joint( + &mut gizmos, + middle_prox.translation, + middle_prox.rotation, + 0.008, + Color::YELLOW, + controller_backward, + hand_translation, + ); + let middle_inter = hand_transform_array[HandJoint::MIDDLE_INTERMEDIATE]; + draw_joint( + &mut gizmos, + middle_inter.translation, + middle_inter.rotation, + 0.006, + Color::YELLOW, + controller_backward, + hand_translation, + ); + let middle_dist = hand_transform_array[HandJoint::MIDDLE_DISTAL]; + draw_joint( + &mut gizmos, + middle_dist.translation, + middle_dist.rotation, + 0.004, + Color::YELLOW, + controller_backward, + hand_translation, + ); + let middle_tip = hand_transform_array[HandJoint::MIDDLE_TIP]; + draw_joint( + &mut gizmos, + middle_tip.translation, + middle_tip.rotation, + 0.002, + Color::YELLOW, + controller_backward, + hand_translation, + ); - let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; - draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, hand_translation); - let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; - draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, hand_translation); - let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; - draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, hand_translation); + let ring_meta = hand_transform_array[HandJoint::RING_METACARPAL]; + draw_joint( + &mut gizmos, + ring_meta.translation, + ring_meta.rotation, + 0.01, + Color::GREEN, + controller_backward, + hand_translation, + ); + let ring_prox = hand_transform_array[HandJoint::RING_PROXIMAL]; + draw_joint( + &mut gizmos, + ring_prox.translation, + ring_prox.rotation, + 0.008, + Color::GREEN, + controller_backward, + hand_translation, + ); + let ring_inter = hand_transform_array[HandJoint::RING_INTERMEDIATE]; + draw_joint( + &mut gizmos, + ring_inter.translation, + ring_inter.rotation, + 0.006, + Color::GREEN, + controller_backward, + hand_translation, + ); + let ring_dist = hand_transform_array[HandJoint::RING_DISTAL]; + draw_joint( + &mut gizmos, + ring_dist.translation, + ring_dist.rotation, + 0.004, + Color::GREEN, + controller_backward, + hand_translation, + ); + let ring_tip = hand_transform_array[HandJoint::RING_TIP]; + draw_joint( + &mut gizmos, + ring_tip.translation, + ring_tip.rotation, + 0.002, + Color::GREEN, + controller_backward, + hand_translation, + ); - let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; - draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, hand_translation); - let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; - draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, hand_translation); - let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; - draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, hand_translation); - let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; - draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, hand_translation); - let index_tip = hand_pose[HandJoint::INDEX_TIP]; - draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, hand_translation); - - let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; - draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, hand_translation); - let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; - draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, hand_translation); - let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; - draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, hand_translation); - let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; - draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, hand_translation); - let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; - draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, hand_translation); - - let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; - draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, hand_translation); - let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; - draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, hand_translation); - let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; - draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, hand_translation); - let ring_dist = hand_pose[HandJoint::RING_DISTAL]; - draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, hand_translation); - let ring_tip = hand_pose[HandJoint::RING_TIP]; - draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, hand_translation); - - let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; - draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, hand_translation); - let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; - draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, hand_translation); - let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; - draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, hand_translation); - let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; - draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, hand_translation); - let little_tip = hand_pose[HandJoint::LITTLE_TIP]; - draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, hand_translation); + let little_meta = hand_transform_array[HandJoint::LITTLE_METACARPAL]; + draw_joint( + &mut gizmos, + little_meta.translation, + little_meta.rotation, + 0.01, + Color::BLUE, + controller_backward, + hand_translation, + ); + let little_prox = hand_transform_array[HandJoint::LITTLE_PROXIMAL]; + draw_joint( + &mut gizmos, + little_prox.translation, + little_prox.rotation, + 0.008, + Color::BLUE, + controller_backward, + hand_translation, + ); + let little_inter = hand_transform_array[HandJoint::LITTLE_INTERMEDIATE]; + draw_joint( + &mut gizmos, + little_inter.translation, + little_inter.rotation, + 0.006, + Color::BLUE, + controller_backward, + hand_translation, + ); + let little_dist = hand_transform_array[HandJoint::LITTLE_DISTAL]; + draw_joint( + &mut gizmos, + little_dist.translation, + little_dist.rotation, + 0.004, + Color::BLUE, + controller_backward, + hand_translation, + ); + let little_tip = hand_transform_array[HandJoint::LITTLE_TIP]; + draw_joint( + &mut gizmos, + little_tip.translation, + little_tip.rotation, + 0.002, + Color::BLUE, + controller_backward, + hand_translation, + ); } fn flip_hand_pose(hand_pose: [Posef; 26], hand: Hand) -> [Posef; 26] { @@ -210,52 +660,164 @@ fn flip_hand_pose(hand_pose: [Posef; 26], hand: Hand) -> [Posef; 26] { for pose in new_pose.iter_mut() { pose.position.x = -pose.position.x; } - }, + } Hand::Right => (), } return new_pose; } -fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, offset: Vec3) { - gizmos.sphere(controller_backwards.mul_vec3(joint_pos) + offset, joint_rot, radius, color); +fn draw_joint( + gizmos: &mut Gizmos, + joint_pos: Vec3, + joint_rot: Quat, + radius: f32, + color: Color, + controller_backwards: Quat, + offset: Vec3, +) { + gizmos.sphere( + controller_backwards.mul_vec3(joint_pos) + offset, + joint_rot, + radius, + color, + ); } fn log_hand(hand_pose: [Posef; 26]) { - let palm_vec = hand_pose[HandJoint::PALM].position.to_vec3(); - info!("palm: {}", hand_pose[HandJoint::PALM].position.to_vec3() - palm_vec); - info!("wrist: {}", hand_pose[HandJoint::WRIST].position.to_vec3() - palm_vec); + 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!("tm: {}", hand_pose[HandJoint::THUMB_METACARPAL].position.to_vec3() - palm_vec); - info!("tp: {}", hand_pose[HandJoint::THUMB_PROXIMAL].position.to_vec3() - palm_vec); - info!("td: {}", hand_pose[HandJoint::THUMB_DISTAL].position.to_vec3() - palm_vec); - info!("tt: {}", hand_pose[HandJoint::THUMB_TIP].position.to_vec3() - palm_vec); - - info!("im: {}", hand_pose[HandJoint::INDEX_METACARPAL].position.to_vec3() - palm_vec); - info!("ip: {}", hand_pose[HandJoint::INDEX_PROXIMAL].position.to_vec3() - palm_vec); - info!("ii: {}", hand_pose[HandJoint::INDEX_INTERMEDIATE].position.to_vec3() - palm_vec); - info!("id: {}", hand_pose[HandJoint::INDEX_DISTAL].position.to_vec3() - palm_vec); - info!("it: {}", hand_pose[HandJoint::INDEX_TIP].position.to_vec3() - palm_vec); + 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!("mm: {}", hand_pose[HandJoint::MIDDLE_METACARPAL].position.to_vec3() - palm_vec); - info!("mp: {}", hand_pose[HandJoint::MIDDLE_PROXIMAL].position.to_vec3() - palm_vec); - info!("mi: {}", hand_pose[HandJoint::MIDDLE_INTERMEDIATE].position.to_vec3() - palm_vec); - info!("md: {}", hand_pose[HandJoint::MIDDLE_DISTAL].position.to_vec3() - palm_vec); - info!("mt: {}", hand_pose[HandJoint::MIDDLE_TIP].position.to_vec3() - palm_vec); + 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!("rm: {}", hand_pose[HandJoint::RING_METACARPAL].position.to_vec3() - palm_vec); - info!("rp: {}", hand_pose[HandJoint::RING_PROXIMAL].position.to_vec3() - palm_vec); - info!("ri: {}", hand_pose[HandJoint::RING_INTERMEDIATE].position.to_vec3() - palm_vec); - info!("rd: {}", hand_pose[HandJoint::RING_DISTAL].position.to_vec3() - palm_vec); - info!("rt: {}", hand_pose[HandJoint::RING_TIP].position.to_vec3() - palm_vec); + 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!("lm: {}", hand_pose[HandJoint::LITTLE_METACARPAL].position.to_vec3() - palm_vec); - info!("lp: {}", hand_pose[HandJoint::LITTLE_PROXIMAL].position.to_vec3() - palm_vec); - info!("li: {}", hand_pose[HandJoint::LITTLE_INTERMEDIATE].position.to_vec3() - palm_vec); - info!("ld: {}", hand_pose[HandJoint::LITTLE_DISTAL].position.to_vec3() - palm_vec); - info!("lt: {}", hand_pose[HandJoint::LITTLE_TIP].position.to_vec3() - palm_vec); + 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() + ); } - fn spawn_controllers_example(mut commands: Commands) { //left hand commands.spawn(( 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/mod.rs b/src/xr_input/mod.rs index 55e7c56..1ee3e71 100644 --- a/src/xr_input/mod.rs +++ b/src/xr_input/mod.rs @@ -4,6 +4,7 @@ pub mod oculus_touch; pub mod prototype_locomotion; pub mod trackers; pub mod xr_camera; +pub mod hand_poses; use crate::resources::XrSession; use crate::xr_begin_frame; From c8ae102ce458b3842eaaac1d79a1b43390d7ed4c Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sat, 7 Oct 2023 18:38:25 -0400 Subject: [PATCH 45/66] hand state resource is used to drive skeleton --- examples/xr.rs | 318 +++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 283 insertions(+), 35 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index d34c4b7..7f65bc4 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -5,8 +5,11 @@ use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::math::vec3; use bevy::transform::components::Transform; use bevy::{gizmos, prelude::*}; +use bevy_openxr::input::XrInput; +use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; use bevy_openxr::xr_input::hand_poses::*; +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, @@ -28,7 +31,9 @@ fn main() { .add_systems(Update, proto_locomotion) .add_systems(Startup, spawn_controllers_example) .add_systems(Update, draw_skeleton_hands) + .add_systems(PreUpdate, update_hand_states) .insert_resource(PrototypeLocomotionConfig::default()) + .insert_resource(HandStatesResource::default()) .run(); } @@ -82,25 +87,144 @@ fn setup( },)); } +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"), + } +} + fn draw_skeleton_hands( mut commands: Commands, mut gizmos: Gizmos, right_controller_query: Query<(&GlobalTransform, With)>, left_controller_query: Query<(&GlobalTransform, With)>, + hand_states_option: Option>, ) { - let left_hand_transform = left_controller_query - .get_single() - .unwrap() - .0 - .compute_transform(); - draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left); - let right_hand_transform = right_controller_query - .get_single() - .unwrap() - .0 - .compute_transform(); - // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); - draw_hand_bones(&mut gizmos, right_hand_transform, Hand::Right); + match hand_states_option { + Some(hands) => { + let left_hand_transform = left_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left, hands.left); + let right_hand_transform = right_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); + draw_hand_bones(&mut gizmos, right_hand_transform, Hand::Right, hands.right); + } + None => info!("hand states resource not initialized yet"), + } } fn pose_array_to_transform_array(hand_pose: [Posef; 26]) -> [Transform; 26] { @@ -114,8 +238,108 @@ fn pose_array_to_transform_array(hand_pose: [Posef; 26]) -> [Transform; 26] { } return result_array; } +#[derive(Clone, Copy)] +pub enum ButtonState { + OFF, + TOUCHED, + PRESSED, +} -fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: Hand) { +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 { + grip: f32, + trigger_state: TriggerState, + a_button: ButtonState, + b_button: ButtonState, + thumbstick: ThumbstickState, +} + +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(), + } + } +} + +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; + } +} + +fn draw_hand_bones( + mut gizmos: &mut Gizmos, + controller_transform: Transform, + hand: Hand, + hand_state: HandState, +) { let left_hand_rot = Quat::from_rotation_y(180.0 * PI / 180.0); let hand_translation: Vec3 = match hand { Hand::Left => controller_transform.translation, @@ -131,26 +355,30 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han Hand::Left => -1.0, Hand::Right => 1.0, }; + //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 paml 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); //draw debug rays - gizmos.ray( - hand_translation, - palm_quat.mul_vec3(Vec3::Z * 0.2), - Color::BLUE, - ); - gizmos.ray( - hand_translation, - palm_quat.mul_vec3(Vec3::Y * 0.2), - Color::GREEN, - ); - gizmos.ray( - hand_translation, - palm_quat.mul_vec3(Vec3::X * 0.2), - Color::RED, - ); + // gizmos.ray( + // hand_translation, + // palm_quat.mul_vec3(Vec3::Z * 0.2), + // Color::BLUE, + // ); + // gizmos.ray( + // hand_translation, + // palm_quat.mul_vec3(Vec3::Y * 0.2), + // Color::GREEN, + // ); + // gizmos.ray( + // hand_translation, + // palm_quat.mul_vec3(Vec3::X * 0.2), + // Color::RED, + // ); //get simulated bones let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); //draw controller-palm bone(should be zero length) @@ -177,11 +405,13 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han let mut prior_vector: Option = None; let color = Color::RED; let splay = Quat::from_rotation_y(splay_direction * 30.0 * PI / 180.0); - let splay_quat = palm_quat.mul_quat(splay); + 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 tp_lrot = Quat::from_rotation_y(splay_direction * 5.0 * PI / 180.0); + 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(); @@ -205,6 +435,7 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han } } + //index //better finger drawing? let thumb_joints = [ HandJoint::INDEX_METACARPAL, @@ -222,7 +453,8 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han for bone in thumb_joints.iter() { match prior_start { Some(start) => { - let tp_lrot = Quat::from_rotation_x(-5.0 * PI / 180.0); + 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(); @@ -263,7 +495,8 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han for bone in thumb_joints.iter() { match prior_start { Some(start) => { - let tp_lrot = Quat::from_rotation_x(-5.0 * PI / 180.0); + 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(); @@ -303,7 +536,8 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han for bone in thumb_joints.iter() { match prior_start { Some(start) => { - let tp_lrot = Quat::from_rotation_x(-5.0 * PI / 180.0); + 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(); @@ -344,7 +578,8 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han for bone in thumb_joints.iter() { match prior_start { Some(start) => { - let tp_lrot = Quat::from_rotation_x(-5.0 * PI / 180.0); + 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(); @@ -369,6 +604,19 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han } } +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 draw_hand(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: Hand) { //draw debug for controller grip center to match palm to let hand_translation = controller_transform.translation; From 4d23856d050237be9c924ab707f1b27bf05132e5 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 00:11:23 -0400 Subject: [PATCH 46/66] spawn bones and update --- examples/xr.rs | 556 ++++++++++++++++++-------------------------- src/xr_input/mod.rs | 4 +- 2 files changed, 231 insertions(+), 329 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 7f65bc4..23de2bd 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -32,6 +32,8 @@ fn main() { .add_systems(Startup, spawn_controllers_example) .add_systems(Update, draw_skeleton_hands) .add_systems(PreUpdate, update_hand_states) + .add_systems(PostUpdate, draw_hand_entities) + .add_systems(Startup, spawn_hand_entities) .insert_resource(PrototypeLocomotionConfig::default()) .insert_resource(HandStatesResource::default()) .run(); @@ -87,6 +89,49 @@ fn setup( },)); } +pub fn spawn_hand_entities(mut commands: Commands) { + let hands = [Hand::Left, Hand::Right]; + let bones = [ + HandBone::PALM, + HandBone::WRIST, + HandBone::THUMB_METACARPAL, + HandBone::THUMB_PROXIMAL, + HandBone::THUMB_DISTAL, + HandBone::THUMB_TIP, + HandBone::INDEX_METACARPAL, + HandBone::INDEX_PROXIMAL, + HandBone::INDEX_INTERMEDIATE, + HandBone::INDEX_DISTAL, + HandBone::INDEX_TIP, + HandBone::MIDDLE_METACARPAL, + HandBone::MIDDLE_PROXIMAL, + HandBone::MIDDLE_INTERMEDIATE, + HandBone::MIDDLE_DISTAL, + HandBone::MIDDLE_TIP, + HandBone::RING_METACARPAL, + HandBone::RING_PROXIMAL, + HandBone::RING_INTERMEDIATE, + HandBone::RING_DISTAL, + HandBone::RING_TIP, + HandBone::LITTLE_METACARPAL, + HandBone::LITTLE_PROXIMAL, + HandBone::LITTLE_INTERMEDIATE, + HandBone::LITTLE_DISTAL, + HandBone::LITTLE_TIP, + ]; + + for hand in hands.iter() { + for bone in bones.iter() { + commands.spawn(( + SpatialBundle::default(), + bone.clone(), + OpenXRTracker, + hand.clone(), + )); + } + } +} + pub fn update_hand_states( oculus_controller: Res, hand_states_option: Option>, @@ -201,11 +246,11 @@ pub fn update_hand_states( } fn draw_skeleton_hands( - mut commands: Commands, mut gizmos: Gizmos, 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)>, ) { match hand_states_option { Some(hands) => { @@ -214,30 +259,61 @@ fn draw_skeleton_hands( .unwrap() .0 .compute_transform(); - draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left, hands.left); + draw_hand_bones( + &mut gizmos, + left_hand_transform, + Hand::Left, + hands.left, + &mut hand_bone_query, + ); let right_hand_transform = right_controller_query .get_single() .unwrap() .0 .compute_transform(); // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); - draw_hand_bones(&mut gizmos, right_hand_transform, Hand::Right, hands.right); + draw_hand_bones( + &mut gizmos, + right_hand_transform, + Hand::Right, + hands.right, + &mut hand_bone_query, + ); } None => info!("hand states resource not initialized yet"), } } -fn pose_array_to_transform_array(hand_pose: [Posef; 26]) -> [Transform; 26] { - let mut result_array: [Transform; 26] = [Transform::default(); 26]; - for (place, data) in result_array.iter_mut().zip(hand_pose.iter()) { - *place = Transform { - translation: data.position.to_vec3(), - rotation: data.orientation.to_quat(), - scale: Vec3::splat(1.0), - } - } - return result_array; +#[derive(Component, Debug, Clone, Copy)] +pub enum HandBone { + PALM, + WRIST, + THUMB_METACARPAL, + THUMB_PROXIMAL, + THUMB_DISTAL, + THUMB_TIP, + INDEX_METACARPAL, + INDEX_PROXIMAL, + INDEX_INTERMEDIATE, + INDEX_DISTAL, + INDEX_TIP, + MIDDLE_METACARPAL, + MIDDLE_PROXIMAL, + MIDDLE_INTERMEDIATE, + MIDDLE_DISTAL, + MIDDLE_TIP, + RING_METACARPAL, + RING_PROXIMAL, + RING_INTERMEDIATE, + RING_DISTAL, + RING_TIP, + LITTLE_METACARPAL, + LITTLE_PROXIMAL, + LITTLE_INTERMEDIATE, + LITTLE_DISTAL, + LITTLE_TIP, } + #[derive(Clone, Copy)] pub enum ButtonState { OFF, @@ -334,11 +410,50 @@ impl HandState { } } +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::THUMB_METACARPAL => (0.01, Color::RED), + HandBone::THUMB_PROXIMAL => (0.008, Color::RED), + HandBone::THUMB_DISTAL => (0.006, Color::RED), + HandBone::THUMB_TIP => (0.004, Color::RED), + HandBone::INDEX_METACARPAL => (0.01, Color::ORANGE), + HandBone::INDEX_PROXIMAL => (0.008, Color::ORANGE), + HandBone::INDEX_INTERMEDIATE => (0.006, Color::ORANGE), + HandBone::INDEX_DISTAL => (0.004, Color::ORANGE), + HandBone::INDEX_TIP => (0.002, Color::ORANGE), + HandBone::MIDDLE_METACARPAL => (0.01, Color::YELLOW), + HandBone::MIDDLE_PROXIMAL => (0.008, Color::YELLOW), + HandBone::MIDDLE_INTERMEDIATE => (0.006, Color::YELLOW), + HandBone::MIDDLE_DISTAL => (0.004, Color::YELLOW), + HandBone::MIDDLE_TIP => (0.002, Color::YELLOW), + HandBone::RING_METACARPAL => (0.01, Color::GREEN), + HandBone::RING_PROXIMAL => (0.008, Color::GREEN), + HandBone::RING_INTERMEDIATE => (0.006, Color::GREEN), + HandBone::RING_DISTAL => (0.004, Color::GREEN), + HandBone::RING_TIP => (0.002, Color::GREEN), + HandBone::LITTLE_METACARPAL => (0.01, Color::BLUE), + HandBone::LITTLE_PROXIMAL => (0.008, Color::BLUE), + HandBone::LITTLE_INTERMEDIATE => (0.006, Color::BLUE), + HandBone::LITTLE_DISTAL => (0.004, Color::BLUE), + HandBone::LITTLE_TIP => (0.002, Color::BLUE), + } +} + fn draw_hand_bones( mut gizmos: &mut Gizmos, 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 { @@ -355,6 +470,9 @@ fn draw_hand_bones( 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(); @@ -384,6 +502,10 @@ fn draw_hand_bones( //draw controller-palm bone(should be zero length) let palm = hand_transform_array[HandJoint::PALM]; gizmos.ray(hand_translation, palm.translation, Color::WHITE); + calc_transforms[HandJoint::PALM] = Transform { + translation: hand_translation + palm.translation, + ..default() + }; //draw palm-wrist let wrist = hand_transform_array[HandJoint::WRIST]; gizmos.ray( @@ -391,6 +513,10 @@ fn draw_hand_bones( palm_quat.mul_vec3(wrist.translation), Color::GRAY, ); + calc_transforms[HandJoint::WRIST] = Transform { + translation: hand_translation + palm.translation + palm_quat.mul_vec3(wrist.translation), + ..default() + }; //thumb //better finger drawing? @@ -420,6 +546,11 @@ fn draw_hand_bones( 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]; @@ -431,6 +562,11 @@ fn draw_hand_bones( 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() + }; } } } @@ -463,6 +599,11 @@ fn draw_hand_bones( 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]; @@ -474,6 +615,11 @@ fn draw_hand_bones( 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() + }; } } } @@ -505,6 +651,11 @@ fn draw_hand_bones( 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]; @@ -516,6 +667,11 @@ fn draw_hand_bones( 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() + }; } } } @@ -546,6 +702,11 @@ fn draw_hand_bones( 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]; @@ -557,6 +718,11 @@ fn draw_hand_bones( 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() + }; } } } @@ -588,6 +754,11 @@ fn draw_hand_bones( 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]; @@ -599,9 +770,54 @@ fn draw_hand_bones( 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::THUMB_METACARPAL => HandJoint::THUMB_METACARPAL, + HandBone::THUMB_PROXIMAL => HandJoint::THUMB_PROXIMAL, + HandBone::THUMB_DISTAL => HandJoint::THUMB_DISTAL, + HandBone::THUMB_TIP => HandJoint::THUMB_TIP, + HandBone::INDEX_METACARPAL => HandJoint::INDEX_METACARPAL, + HandBone::INDEX_PROXIMAL => HandJoint::INDEX_PROXIMAL, + HandBone::INDEX_INTERMEDIATE => HandJoint::INDEX_INTERMEDIATE, + HandBone::INDEX_DISTAL => HandJoint::INDEX_DISTAL, + HandBone::INDEX_TIP => HandJoint::INDEX_TIP, + HandBone::MIDDLE_METACARPAL => HandJoint::MIDDLE_METACARPAL, + HandBone::MIDDLE_PROXIMAL => HandJoint::MIDDLE_PROXIMAL, + HandBone::MIDDLE_INTERMEDIATE => HandJoint::MIDDLE_INTERMEDIATE, + HandBone::MIDDLE_DISTAL => HandJoint::MIDDLE_DISTAL, + HandBone::MIDDLE_TIP => HandJoint::MIDDLE_TIP, + HandBone::RING_METACARPAL => HandJoint::RING_METACARPAL, + HandBone::RING_PROXIMAL => HandJoint::RING_PROXIMAL, + HandBone::RING_INTERMEDIATE => HandJoint::RING_INTERMEDIATE, + HandBone::RING_DISTAL => HandJoint::RING_DISTAL, + HandBone::RING_TIP => HandJoint::RING_TIP, + HandBone::LITTLE_METACARPAL => HandJoint::LITTLE_METACARPAL, + HandBone::LITTLE_PROXIMAL => HandJoint::LITTLE_PROXIMAL, + HandBone::LITTLE_INTERMEDIATE => HandJoint::LITTLE_INTERMEDIATE, + HandBone::LITTLE_DISTAL => HandJoint::LITTLE_DISTAL, + HandBone::LITTLE_TIP => HandJoint::LITTLE_TIP, + } } fn get_bone_curl_angle(bone: HandJoint, curl: f32) -> f32 { @@ -617,320 +833,6 @@ fn get_bone_curl_angle(bone: HandJoint, curl: f32) -> f32 { return curl_angle; } -fn draw_hand(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: Hand) { - //draw debug for controller grip center to match palm to - let hand_translation = controller_transform.translation; - let hand_quat = controller_transform.rotation; - //we need to flip this i dont know why - let flip = Quat::from_rotation_x(PI); - let controller_backward = hand_quat.mul_quat(flip); - - let test_hand_pose = get_test_hand_pose_array(); - - let hand_pose = flip_hand_pose(test_hand_pose.clone(), hand); - - // let hand_transform_array: [Transform; 26] = pose_array_to_transform_array(hand_pose); - let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); - - let palm = hand_transform_array[HandJoint::PALM]; - gizmos.sphere( - palm.translation + hand_translation, - palm.rotation.mul_quat(controller_backward), - 0.01, - Color::WHITE, - ); - - let wrist = hand_transform_array[HandJoint::WRIST]; - draw_joint( - &mut gizmos, - wrist.translation, - wrist.rotation, - 0.01, - Color::GRAY, - controller_backward, - hand_translation, - ); - - let thumb_meta = hand_transform_array[HandJoint::THUMB_METACARPAL]; - draw_joint( - &mut gizmos, - thumb_meta.translation, - thumb_meta.rotation, - 0.01, - Color::RED, - controller_backward, - hand_translation, - ); - - let thumb_prox = hand_transform_array[HandJoint::THUMB_PROXIMAL]; - draw_joint( - &mut gizmos, - thumb_prox.translation, - thumb_prox.rotation, - 0.008, - Color::RED, - controller_backward, - hand_translation, - ); - let thumb_dist = hand_transform_array[HandJoint::THUMB_DISTAL]; - draw_joint( - &mut gizmos, - thumb_dist.translation, - thumb_dist.rotation, - 0.006, - Color::RED, - controller_backward, - hand_translation, - ); - let thumb_tip = hand_transform_array[HandJoint::THUMB_TIP]; - draw_joint( - &mut gizmos, - thumb_tip.translation, - thumb_tip.rotation, - 0.004, - Color::RED, - controller_backward, - hand_translation, - ); - - let index_meta = hand_transform_array[HandJoint::INDEX_METACARPAL]; - draw_joint( - &mut gizmos, - index_meta.translation, - index_meta.rotation, - 0.01, - Color::ORANGE, - controller_backward, - hand_translation, - ); - let index_prox = hand_transform_array[HandJoint::INDEX_PROXIMAL]; - draw_joint( - &mut gizmos, - index_prox.translation, - index_prox.rotation, - 0.008, - Color::ORANGE, - controller_backward, - hand_translation, - ); - - let index_inter = hand_transform_array[HandJoint::INDEX_INTERMEDIATE]; - draw_joint( - &mut gizmos, - index_inter.translation, - index_inter.rotation, - 0.006, - Color::ORANGE, - controller_backward, - hand_translation, - ); - - let index_dist = hand_transform_array[HandJoint::INDEX_DISTAL]; - draw_joint( - &mut gizmos, - index_dist.translation, - index_dist.rotation, - 0.004, - Color::ORANGE, - controller_backward, - hand_translation, - ); - - let index_tip = hand_transform_array[HandJoint::INDEX_TIP]; - draw_joint( - &mut gizmos, - index_tip.translation, - index_tip.rotation, - 0.002, - Color::ORANGE, - controller_backward, - hand_translation, - ); - - let middle_meta = hand_transform_array[HandJoint::MIDDLE_METACARPAL]; - draw_joint( - &mut gizmos, - middle_meta.translation, - middle_meta.rotation, - 0.01, - Color::YELLOW, - controller_backward, - hand_translation, - ); - let middle_prox = hand_transform_array[HandJoint::MIDDLE_PROXIMAL]; - draw_joint( - &mut gizmos, - middle_prox.translation, - middle_prox.rotation, - 0.008, - Color::YELLOW, - controller_backward, - hand_translation, - ); - let middle_inter = hand_transform_array[HandJoint::MIDDLE_INTERMEDIATE]; - draw_joint( - &mut gizmos, - middle_inter.translation, - middle_inter.rotation, - 0.006, - Color::YELLOW, - controller_backward, - hand_translation, - ); - let middle_dist = hand_transform_array[HandJoint::MIDDLE_DISTAL]; - draw_joint( - &mut gizmos, - middle_dist.translation, - middle_dist.rotation, - 0.004, - Color::YELLOW, - controller_backward, - hand_translation, - ); - let middle_tip = hand_transform_array[HandJoint::MIDDLE_TIP]; - draw_joint( - &mut gizmos, - middle_tip.translation, - middle_tip.rotation, - 0.002, - Color::YELLOW, - controller_backward, - hand_translation, - ); - - let ring_meta = hand_transform_array[HandJoint::RING_METACARPAL]; - draw_joint( - &mut gizmos, - ring_meta.translation, - ring_meta.rotation, - 0.01, - Color::GREEN, - controller_backward, - hand_translation, - ); - let ring_prox = hand_transform_array[HandJoint::RING_PROXIMAL]; - draw_joint( - &mut gizmos, - ring_prox.translation, - ring_prox.rotation, - 0.008, - Color::GREEN, - controller_backward, - hand_translation, - ); - let ring_inter = hand_transform_array[HandJoint::RING_INTERMEDIATE]; - draw_joint( - &mut gizmos, - ring_inter.translation, - ring_inter.rotation, - 0.006, - Color::GREEN, - controller_backward, - hand_translation, - ); - let ring_dist = hand_transform_array[HandJoint::RING_DISTAL]; - draw_joint( - &mut gizmos, - ring_dist.translation, - ring_dist.rotation, - 0.004, - Color::GREEN, - controller_backward, - hand_translation, - ); - let ring_tip = hand_transform_array[HandJoint::RING_TIP]; - draw_joint( - &mut gizmos, - ring_tip.translation, - ring_tip.rotation, - 0.002, - Color::GREEN, - controller_backward, - hand_translation, - ); - - let little_meta = hand_transform_array[HandJoint::LITTLE_METACARPAL]; - draw_joint( - &mut gizmos, - little_meta.translation, - little_meta.rotation, - 0.01, - Color::BLUE, - controller_backward, - hand_translation, - ); - let little_prox = hand_transform_array[HandJoint::LITTLE_PROXIMAL]; - draw_joint( - &mut gizmos, - little_prox.translation, - little_prox.rotation, - 0.008, - Color::BLUE, - controller_backward, - hand_translation, - ); - let little_inter = hand_transform_array[HandJoint::LITTLE_INTERMEDIATE]; - draw_joint( - &mut gizmos, - little_inter.translation, - little_inter.rotation, - 0.006, - Color::BLUE, - controller_backward, - hand_translation, - ); - let little_dist = hand_transform_array[HandJoint::LITTLE_DISTAL]; - draw_joint( - &mut gizmos, - little_dist.translation, - little_dist.rotation, - 0.004, - Color::BLUE, - controller_backward, - hand_translation, - ); - let little_tip = hand_transform_array[HandJoint::LITTLE_TIP]; - draw_joint( - &mut gizmos, - little_tip.translation, - little_tip.rotation, - 0.002, - Color::BLUE, - controller_backward, - hand_translation, - ); -} - -fn flip_hand_pose(hand_pose: [Posef; 26], hand: Hand) -> [Posef; 26] { - let mut new_pose = hand_pose; - match hand { - Hand::Left => { - for pose in new_pose.iter_mut() { - pose.position.x = -pose.position.x; - } - } - Hand::Right => (), - } - return new_pose; -} - -fn draw_joint( - gizmos: &mut Gizmos, - joint_pos: Vec3, - joint_rot: Quat, - radius: f32, - color: Color, - controller_backwards: Quat, - offset: Vec3, -) { - gizmos.sphere( - controller_backwards.mul_vec3(joint_pos) + offset, - joint_rot, - radius, - color, - ); -} - fn log_hand(hand_pose: [Posef; 26]) { let palm_wrist = hand_pose[HandJoint::WRIST].position.to_vec3() - hand_pose[HandJoint::PALM].position.to_vec3(); diff --git a/src/xr_input/mod.rs b/src/xr_input/mod.rs index 1ee3e71..a81badc 100644 --- a/src/xr_input/mod.rs +++ b/src/xr_input/mod.rs @@ -13,7 +13,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}; @@ -28,7 +28,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, From 4f30d9e5296f04420fabba47facee34c9c65980d Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 00:16:14 -0400 Subject: [PATCH 47/66] slight cleaning --- examples/xr.rs | 66 +++++++++----------------------------------------- 1 file changed, 11 insertions(+), 55 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 23de2bd..91f6587 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -30,7 +30,7 @@ fn main() { .add_systems(Startup, setup) .add_systems(Update, proto_locomotion) .add_systems(Startup, spawn_controllers_example) - .add_systems(Update, draw_skeleton_hands) + .add_systems(Update, update_emulated_hand_skeletons) .add_systems(PreUpdate, update_hand_states) .add_systems(PostUpdate, draw_hand_entities) .add_systems(Startup, spawn_hand_entities) @@ -245,8 +245,7 @@ pub fn update_hand_states( } } -fn draw_skeleton_hands( - mut gizmos: Gizmos, +fn update_emulated_hand_skeletons( right_controller_query: Query<(&GlobalTransform, With)>, left_controller_query: Query<(&GlobalTransform, With)>, hand_states_option: Option>, @@ -259,8 +258,7 @@ fn draw_skeleton_hands( .unwrap() .0 .compute_transform(); - draw_hand_bones( - &mut gizmos, + update_hand_bones_emulated( left_hand_transform, Hand::Left, hands.left, @@ -271,9 +269,7 @@ fn draw_skeleton_hands( .unwrap() .0 .compute_transform(); - // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); - draw_hand_bones( - &mut gizmos, + update_hand_bones_emulated( right_hand_transform, Hand::Right, hands.right, @@ -448,8 +444,7 @@ fn get_bone_gizmo_style(hand_bone: &HandBone) -> (f32, Color) { } } -fn draw_hand_bones( - mut gizmos: &mut Gizmos, +fn update_hand_bones_emulated( controller_transform: Transform, hand: Hand, hand_state: HandState, @@ -477,49 +472,26 @@ fn draw_hand_bones( let grip_curl = hand_state.grip; let index_curl = hand_state.get_index_curl(); let thumb_curl = hand_state.get_thumb_curl(); - //get paml quat + //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); - //draw debug rays - // gizmos.ray( - // hand_translation, - // palm_quat.mul_vec3(Vec3::Z * 0.2), - // Color::BLUE, - // ); - // gizmos.ray( - // hand_translation, - // palm_quat.mul_vec3(Vec3::Y * 0.2), - // Color::GREEN, - // ); - // gizmos.ray( - // hand_translation, - // palm_quat.mul_vec3(Vec3::X * 0.2), - // Color::RED, - // ); //get simulated bones let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); - //draw controller-palm bone(should be zero length) + //palm let palm = hand_transform_array[HandJoint::PALM]; - gizmos.ray(hand_translation, palm.translation, Color::WHITE); calc_transforms[HandJoint::PALM] = Transform { translation: hand_translation + palm.translation, ..default() }; - //draw palm-wrist + //wrist let wrist = hand_transform_array[HandJoint::WRIST]; - gizmos.ray( - hand_translation + palm.translation, - palm_quat.mul_vec3(wrist.translation), - Color::GRAY, - ); calc_transforms[HandJoint::WRIST] = Transform { translation: hand_translation + palm.translation + palm_quat.mul_vec3(wrist.translation), ..default() }; //thumb - //better finger drawing? let thumb_joints = [ HandJoint::THUMB_METACARPAL, HandJoint::THUMB_PROXIMAL, @@ -529,7 +501,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::RED; 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); @@ -542,7 +513,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -558,7 +528,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); @@ -572,7 +541,6 @@ fn draw_hand_bones( } //index - //better finger drawing? let thumb_joints = [ HandJoint::INDEX_METACARPAL, HandJoint::INDEX_PROXIMAL, @@ -583,7 +551,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::ORANGE; 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() { @@ -595,7 +562,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -611,7 +577,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); @@ -624,7 +589,7 @@ fn draw_hand_bones( } } - //better finger drawing? + //middle let thumb_joints = [ HandJoint::MIDDLE_METACARPAL, HandJoint::MIDDLE_PROXIMAL, @@ -635,7 +600,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::YELLOW; 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() { @@ -647,7 +611,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -663,7 +626,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); @@ -675,7 +637,7 @@ fn draw_hand_bones( } } } - //better finger drawing? + //ring let thumb_joints = [ HandJoint::RING_METACARPAL, HandJoint::RING_PROXIMAL, @@ -686,7 +648,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::GREEN; 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() { @@ -698,7 +659,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -714,7 +674,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); @@ -727,7 +686,7 @@ fn draw_hand_bones( } } - //better finger drawing? + //little let thumb_joints = [ HandJoint::LITTLE_METACARPAL, HandJoint::LITTLE_PROXIMAL, @@ -738,7 +697,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::BLUE; 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() { @@ -750,7 +708,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -766,7 +723,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); From fdbd53fba169199f0fca05d9262eb96f1157acfa Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 00:17:31 -0400 Subject: [PATCH 48/66] cargo fix --- examples/xr.rs | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 91f6587..ad738a9 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,22 +1,22 @@ use std::f32::consts::PI; -use std::ops::Mul; + use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; -use bevy::math::vec3; + use bevy::transform::components::Transform; -use bevy::{gizmos, prelude::*}; +use bevy::{prelude::*}; use bevy_openxr::input::XrInput; use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; -use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; + use bevy_openxr::xr_input::hand_poses::*; 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, }; -use bevy_openxr::xr_input::{Hand, QuatConv, Vec3Conv}; +use bevy_openxr::xr_input::{Hand, Vec3Conv}; use bevy_openxr::DefaultXrPlugins; -use openxr::{HandJoint, Posef, Quaternionf, Vector3f}; +use openxr::{HandJoint, Posef}; fn main() { color_eyre::install().unwrap(); @@ -790,7 +790,7 @@ fn get_bone_curl_angle(bone: HandJoint, curl: f32) -> f32 { } fn log_hand(hand_pose: [Posef; 26]) { - let palm_wrist = hand_pose[HandJoint::WRIST].position.to_vec3() + let _palm_wrist = hand_pose[HandJoint::WRIST].position.to_vec3() - hand_pose[HandJoint::PALM].position.to_vec3(); info!( "palm-wrist: {}", From 166fe1a2c075e7e8f450a652ca918e702784d0c5 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 00:19:43 -0400 Subject: [PATCH 49/66] mass rename --- examples/xr.rs | 210 ++++++++++++++++++++++++------------------------- 1 file changed, 105 insertions(+), 105 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index ad738a9..d47b6f8 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -4,7 +4,7 @@ use std::f32::consts::PI; use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::transform::components::Transform; -use bevy::{prelude::*}; +use bevy::prelude::*; use bevy_openxr::input::XrInput; use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; @@ -92,32 +92,32 @@ fn setup( pub fn spawn_hand_entities(mut commands: Commands) { let hands = [Hand::Left, Hand::Right]; let bones = [ - HandBone::PALM, - HandBone::WRIST, - HandBone::THUMB_METACARPAL, - HandBone::THUMB_PROXIMAL, - HandBone::THUMB_DISTAL, - HandBone::THUMB_TIP, - HandBone::INDEX_METACARPAL, - HandBone::INDEX_PROXIMAL, - HandBone::INDEX_INTERMEDIATE, - HandBone::INDEX_DISTAL, - HandBone::INDEX_TIP, - HandBone::MIDDLE_METACARPAL, - HandBone::MIDDLE_PROXIMAL, - HandBone::MIDDLE_INTERMEDIATE, - HandBone::MIDDLE_DISTAL, - HandBone::MIDDLE_TIP, - HandBone::RING_METACARPAL, - HandBone::RING_PROXIMAL, - HandBone::RING_INTERMEDIATE, - HandBone::RING_DISTAL, - HandBone::RING_TIP, - HandBone::LITTLE_METACARPAL, - HandBone::LITTLE_PROXIMAL, - HandBone::LITTLE_INTERMEDIATE, - HandBone::LITTLE_DISTAL, - HandBone::LITTLE_TIP, + 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, ]; for hand in hands.iter() { @@ -282,32 +282,32 @@ fn update_emulated_hand_skeletons( #[derive(Component, Debug, Clone, Copy)] pub enum HandBone { - PALM, - WRIST, - THUMB_METACARPAL, - THUMB_PROXIMAL, - THUMB_DISTAL, - THUMB_TIP, - INDEX_METACARPAL, - INDEX_PROXIMAL, - INDEX_INTERMEDIATE, - INDEX_DISTAL, - INDEX_TIP, - MIDDLE_METACARPAL, - MIDDLE_PROXIMAL, - MIDDLE_INTERMEDIATE, - MIDDLE_DISTAL, - MIDDLE_TIP, - RING_METACARPAL, - RING_PROXIMAL, - RING_INTERMEDIATE, - RING_DISTAL, - RING_TIP, - LITTLE_METACARPAL, - LITTLE_PROXIMAL, - LITTLE_INTERMEDIATE, - LITTLE_DISTAL, - LITTLE_TIP, + 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, } #[derive(Clone, Copy)] @@ -415,32 +415,32 @@ fn draw_hand_entities(mut gizmos: Gizmos, query: Query<(&Transform, &HandBone)>) 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::THUMB_METACARPAL => (0.01, Color::RED), - HandBone::THUMB_PROXIMAL => (0.008, Color::RED), - HandBone::THUMB_DISTAL => (0.006, Color::RED), - HandBone::THUMB_TIP => (0.004, Color::RED), - HandBone::INDEX_METACARPAL => (0.01, Color::ORANGE), - HandBone::INDEX_PROXIMAL => (0.008, Color::ORANGE), - HandBone::INDEX_INTERMEDIATE => (0.006, Color::ORANGE), - HandBone::INDEX_DISTAL => (0.004, Color::ORANGE), - HandBone::INDEX_TIP => (0.002, Color::ORANGE), - HandBone::MIDDLE_METACARPAL => (0.01, Color::YELLOW), - HandBone::MIDDLE_PROXIMAL => (0.008, Color::YELLOW), - HandBone::MIDDLE_INTERMEDIATE => (0.006, Color::YELLOW), - HandBone::MIDDLE_DISTAL => (0.004, Color::YELLOW), - HandBone::MIDDLE_TIP => (0.002, Color::YELLOW), - HandBone::RING_METACARPAL => (0.01, Color::GREEN), - HandBone::RING_PROXIMAL => (0.008, Color::GREEN), - HandBone::RING_INTERMEDIATE => (0.006, Color::GREEN), - HandBone::RING_DISTAL => (0.004, Color::GREEN), - HandBone::RING_TIP => (0.002, Color::GREEN), - HandBone::LITTLE_METACARPAL => (0.01, Color::BLUE), - HandBone::LITTLE_PROXIMAL => (0.008, Color::BLUE), - HandBone::LITTLE_INTERMEDIATE => (0.006, Color::BLUE), - HandBone::LITTLE_DISTAL => (0.004, Color::BLUE), - HandBone::LITTLE_TIP => (0.002, Color::BLUE), + 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), } } @@ -747,32 +747,32 @@ fn update_hand_bones_emulated( fn match_index(handbone: &HandBone) -> HandJoint { match handbone { - HandBone::PALM => HandJoint::PALM, - HandBone::WRIST => HandJoint::WRIST, - HandBone::THUMB_METACARPAL => HandJoint::THUMB_METACARPAL, - HandBone::THUMB_PROXIMAL => HandJoint::THUMB_PROXIMAL, - HandBone::THUMB_DISTAL => HandJoint::THUMB_DISTAL, - HandBone::THUMB_TIP => HandJoint::THUMB_TIP, - HandBone::INDEX_METACARPAL => HandJoint::INDEX_METACARPAL, - HandBone::INDEX_PROXIMAL => HandJoint::INDEX_PROXIMAL, - HandBone::INDEX_INTERMEDIATE => HandJoint::INDEX_INTERMEDIATE, - HandBone::INDEX_DISTAL => HandJoint::INDEX_DISTAL, - HandBone::INDEX_TIP => HandJoint::INDEX_TIP, - HandBone::MIDDLE_METACARPAL => HandJoint::MIDDLE_METACARPAL, - HandBone::MIDDLE_PROXIMAL => HandJoint::MIDDLE_PROXIMAL, - HandBone::MIDDLE_INTERMEDIATE => HandJoint::MIDDLE_INTERMEDIATE, - HandBone::MIDDLE_DISTAL => HandJoint::MIDDLE_DISTAL, - HandBone::MIDDLE_TIP => HandJoint::MIDDLE_TIP, - HandBone::RING_METACARPAL => HandJoint::RING_METACARPAL, - HandBone::RING_PROXIMAL => HandJoint::RING_PROXIMAL, - HandBone::RING_INTERMEDIATE => HandJoint::RING_INTERMEDIATE, - HandBone::RING_DISTAL => HandJoint::RING_DISTAL, - HandBone::RING_TIP => HandJoint::RING_TIP, - HandBone::LITTLE_METACARPAL => HandJoint::LITTLE_METACARPAL, - HandBone::LITTLE_PROXIMAL => HandJoint::LITTLE_PROXIMAL, - HandBone::LITTLE_INTERMEDIATE => HandJoint::LITTLE_INTERMEDIATE, - HandBone::LITTLE_DISTAL => HandJoint::LITTLE_DISTAL, - HandBone::LITTLE_TIP => HandJoint::LITTLE_TIP, + 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, } } From 92eb5736eb6e2b28cef5186ba1f445cee992e239 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 21:04:20 -0400 Subject: [PATCH 50/66] create resource to help with entity selection --- examples/xr.rs | 80 +++++++++++++++++++++++++++++++++++----- src/xr_input/hand.rs | 88 ++++++++++++++++++++++++++++++++++++++++++-- src/xr_input/mod.rs | 1 + 3 files changed, 157 insertions(+), 12 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index d47b6f8..4fb94bb 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,13 +1,13 @@ use std::f32::consts::PI; - use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; -use bevy::transform::components::Transform; use bevy::prelude::*; +use bevy::transform::components::Transform; use bevy_openxr::input::XrInput; use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; +use bevy_openxr::xr_input::hand::HandsResource; use bevy_openxr::xr_input::hand_poses::*; use bevy_openxr::xr_input::oculus_touch::OculusController; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; @@ -119,17 +119,79 @@ pub fn spawn_hand_entities(mut commands: Commands) { HandBone::LittleDistal, HandBone::LittleTip, ]; - + //hand resource + let mut hand_resource = HandsResource { ..default() }; for hand in hands.iter() { for bone in bones.iter() { - commands.spawn(( - SpatialBundle::default(), - bone.clone(), - OpenXRTracker, - hand.clone(), - )); + 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); } pub fn update_hand_states( diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs index 0fcd267..b84e0dc 100644 --- a/src/xr_input/hand.rs +++ b/src/xr_input/hand.rs @@ -1,3 +1,85 @@ -pub struct Hand { - pub HandJointPoses: [Posef; 26] -} \ No newline at end of file +use bevy::prelude::{Entity, Resource}; + +#[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 } + } +} diff --git a/src/xr_input/mod.rs b/src/xr_input/mod.rs index a81badc..65eef19 100644 --- a/src/xr_input/mod.rs +++ b/src/xr_input/mod.rs @@ -5,6 +5,7 @@ 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; From 89f0e5367db2abc296d6ec3078048049a090b072 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 10 Oct 2023 13:05:09 -0400 Subject: [PATCH 51/66] moved everything to the hand file --- examples/xr.rs | 909 +------------------------------------------ src/xr_input/hand.rs | 907 +++++++++++++++++++++++++++++++++++++++++- 2 files changed, 909 insertions(+), 907 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 4fb94bb..cbc7ac7 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,22 +1,16 @@ -use std::f32::consts::PI; + use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; -use bevy_openxr::input::XrInput; -use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; -use bevy_openxr::xr_input::hand::HandsResource; -use bevy_openxr::xr_input::hand_poses::*; -use bevy_openxr::xr_input::oculus_touch::OculusController; +use bevy_openxr::xr_input::hand::{spawn_hand_entities, update_hand_states, HandStatesResource, update_emulated_hand_skeletons, draw_hand_entities}; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, }; -use bevy_openxr::xr_input::{Hand, Vec3Conv}; use bevy_openxr::DefaultXrPlugins; -use openxr::{HandJoint, Posef}; fn main() { color_eyre::install().unwrap(); @@ -89,903 +83,6 @@ fn setup( },)); } -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); -} - -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"), - } -} - -fn update_emulated_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)>, -) { - 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"), - } -} - -#[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, -} - -#[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 { - grip: f32, - trigger_state: TriggerState, - a_button: ButtonState, - b_button: ButtonState, - thumbstick: ThumbstickState, -} - -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(), - } - } -} - -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; - } -} - -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), - } -} - -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() - ); -} - fn spawn_controllers_example(mut commands: Commands) { //left hand commands.spawn(( @@ -1001,4 +98,4 @@ fn spawn_controllers_example(mut commands: Commands) { OpenXRTracker, SpatialBundle::default(), )); -} +} \ No newline at end of file diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs index b84e0dc..f02b5c4 100644 --- a/src/xr_input/hand.rs +++ b/src/xr_input/hand.rs @@ -1,4 +1,11 @@ -use bevy::prelude::{Entity, Resource}; +use std::f32::consts::PI; + +use bevy::prelude::{Entity, Resource, Component, default, SpatialBundle, Commands, Res, ResMut, info, Transform, Query, Quat, Vec3, GlobalTransform, With, Gizmos, Color}; +use openxr::{HandJoint, Posef}; + +use crate::{resources::{XrFrameState, XrInstance, XrSession}, input::XrInput, xr_input::Vec3Conv}; + +use super::{trackers::{OpenXRTracker, OpenXRRightController, OpenXRLeftController}, Hand, oculus_touch::OculusController, hand_poses::get_simulated_open_hand_transforms}; #[derive(Resource, Default)] pub struct HandsResource { @@ -83,3 +90,901 @@ impl Default for LittleResource { 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_emulated_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)>, +) { + 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"), + } +} + +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), + } +} \ No newline at end of file From 1d434bb7a1e405cf9653a4dc4557972dbd769d7d Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sat, 14 Oct 2023 11:02:49 -0400 Subject: [PATCH 52/66] look upon my bad code --- examples/xr.rs | 10 +-- src/xr_input/hand.rs | 174 +++++++++++++++++++++++++++++++++---------- 2 files changed, 138 insertions(+), 46 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index cbc7ac7..363cbe9 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -5,7 +5,7 @@ use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; -use bevy_openxr::xr_input::hand::{spawn_hand_entities, update_hand_states, HandStatesResource, update_emulated_hand_skeletons, draw_hand_entities}; +use bevy_openxr::xr_input::hand::OpenXrHandInput; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, @@ -23,13 +23,9 @@ fn main() { .add_plugins(FrameTimeDiagnosticsPlugin) .add_systems(Startup, setup) .add_systems(Update, proto_locomotion) - .add_systems(Startup, spawn_controllers_example) - .add_systems(Update, update_emulated_hand_skeletons) - .add_systems(PreUpdate, update_hand_states) - .add_systems(PostUpdate, draw_hand_entities) - .add_systems(Startup, spawn_hand_entities) .insert_resource(PrototypeLocomotionConfig::default()) - .insert_resource(HandStatesResource::default()) + .add_systems(Startup, spawn_controllers_example) + .add_plugins(OpenXrHandInput) .run(); } diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs index f02b5c4..63e5ef8 100644 --- a/src/xr_input/hand.rs +++ b/src/xr_input/hand.rs @@ -1,11 +1,51 @@ use std::f32::consts::PI; -use bevy::prelude::{Entity, Resource, Component, default, SpatialBundle, Commands, Res, ResMut, info, Transform, Query, Quat, Vec3, GlobalTransform, With, Gizmos, Color}; +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::{resources::{XrFrameState, XrInstance, XrSession}, input::XrInput, xr_input::Vec3Conv}; +use crate::{ + input::XrInput, + resources::{XrFrameState, XrInstance, XrSession}, + xr_input::Vec3Conv, +}; -use super::{trackers::{OpenXRTracker, OpenXRRightController, OpenXRLeftController}, Hand, oculus_touch::OculusController, hand_poses::get_simulated_open_hand_transforms}; +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(PostUpdate, draw_hand_entities) + .add_systems(Startup, spawn_hand_entities) + .insert_resource(HandStatesResource::default()) + .insert_resource(HandInputSource::default()); + } +} + +#[derive(Resource)] +pub enum HandInputSource { + Emulated, + OpenXr, +} + +impl Default for HandInputSource { + fn default() -> Self { + HandInputSource::Emulated + } +} #[derive(Resource, Default)] pub struct HandsResource { @@ -25,7 +65,15 @@ pub struct HandResource { 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() } + Self { + palm: Entity::PLACEHOLDER, + wrist: Entity::PLACEHOLDER, + thumb: Default::default(), + index: Default::default(), + middle: Default::default(), + ring: Default::default(), + little: Default::default(), + } } } @@ -38,7 +86,12 @@ pub struct ThumbResource { impl Default for ThumbResource { fn default() -> Self { - Self { metacarpal: Entity::PLACEHOLDER, proximal: Entity::PLACEHOLDER, distal: Entity::PLACEHOLDER, tip: Entity::PLACEHOLDER } + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } } } pub struct IndexResource { @@ -51,7 +104,13 @@ pub struct IndexResource { impl Default for IndexResource { fn default() -> Self { - Self { metacarpal: Entity::PLACEHOLDER, proximal: Entity::PLACEHOLDER, intermediate: Entity::PLACEHOLDER, distal: Entity::PLACEHOLDER, tip: Entity::PLACEHOLDER } + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + intermediate: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } } } pub struct MiddleResource { @@ -63,7 +122,13 @@ pub struct MiddleResource { } impl Default for MiddleResource { fn default() -> Self { - Self { metacarpal: Entity::PLACEHOLDER, proximal: Entity::PLACEHOLDER, intermediate: Entity::PLACEHOLDER, distal: Entity::PLACEHOLDER, tip: Entity::PLACEHOLDER } + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + intermediate: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } } } pub struct RingResource { @@ -75,7 +140,13 @@ pub struct RingResource { } impl Default for RingResource { fn default() -> Self { - Self { metacarpal: Entity::PLACEHOLDER, proximal: Entity::PLACEHOLDER, intermediate: Entity::PLACEHOLDER, distal: Entity::PLACEHOLDER, tip: Entity::PLACEHOLDER } + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + intermediate: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } } } pub struct LittleResource { @@ -87,7 +158,13 @@ pub struct LittleResource { } impl Default for LittleResource { fn default() -> Self { - Self { metacarpal: Entity::PLACEHOLDER, proximal: Entity::PLACEHOLDER, intermediate: Entity::PLACEHOLDER, distal: Entity::PLACEHOLDER, tip: Entity::PLACEHOLDER } + Self { + metacarpal: Entity::PLACEHOLDER, + proximal: Entity::PLACEHOLDER, + intermediate: Entity::PLACEHOLDER, + distal: Entity::PLACEHOLDER, + tip: Entity::PLACEHOLDER, + } } } @@ -176,7 +253,9 @@ pub fn spawn_hand_entities(mut commands: Commands) { 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::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, @@ -186,7 +265,9 @@ pub fn spawn_hand_entities(mut commands: Commands) { 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::LittleIntermediate => { + hand_resource.right.little.intermediate = boneid + } HandBone::LittleDistal => hand_resource.right.little.distal = boneid, HandBone::LittleTip => hand_resource.right.little.tip = boneid, }, @@ -915,39 +996,54 @@ fn log_hand(hand_pose: [Posef; 26]) { ); } - -pub fn update_emulated_hand_skeletons( +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 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, - ); + 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; } - None => info!("hand states resource not initialized yet"), } } @@ -987,4 +1083,4 @@ fn get_bone_gizmo_style(hand_bone: &HandBone) -> (f32, Color) { HandBone::LittleDistal => (0.004, Color::BLUE), HandBone::LittleTip => (0.002, Color::BLUE), } -} \ No newline at end of file +} From 6e7f158dd54fe9d2bbbe5c68cadac87d1b390b81 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sat, 14 Oct 2023 11:05:06 -0400 Subject: [PATCH 53/66] pulled debug rendering into its own plugin --- examples/xr.rs | 3 ++- src/xr_input/hand.rs | 11 ++++++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 363cbe9..f0d468d 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -5,7 +5,7 @@ use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; -use bevy_openxr::xr_input::hand::OpenXrHandInput; +use bevy_openxr::xr_input::hand::{OpenXrHandInput, HandInputDebugRenderer}; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, @@ -26,6 +26,7 @@ fn main() { .insert_resource(PrototypeLocomotionConfig::default()) .add_systems(Startup, spawn_controllers_example) .add_plugins(OpenXrHandInput) + .add_plugins(HandInputDebugRenderer) .run(); } diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs index 63e5ef8..f8a70ac 100644 --- a/src/xr_input/hand.rs +++ b/src/xr_input/hand.rs @@ -28,13 +28,22 @@ 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(PostUpdate, draw_hand_entities) .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, From fd1a3efa0bc521cd10b30b50abaed93fe5aac943 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 26 Sep 2023 12:54:52 -0400 Subject: [PATCH 54/66] got a hand array --- examples/xr.rs | 102 +++++++++++++++++++++++++++++++++++++++++++ src/xr_input/hand.rs | 2 +- 2 files changed, 103 insertions(+), 1 deletion(-) diff --git a/examples/xr.rs b/examples/xr.rs index f0d468d..79e4727 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -5,12 +5,14 @@ use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; +use bevy_openxr::xr_input::{QuatConv, Vec3Conv}; use bevy_openxr::xr_input::hand::{OpenXrHandInput, HandInputDebugRenderer}; use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, }; use bevy_openxr::DefaultXrPlugins; +use openxr::{Posef, Quaternionf, Vector3f, HandJoint}; fn main() { color_eyre::install().unwrap(); @@ -80,6 +82,106 @@ fn setup( },)); } +fn spawn_skeleton(mut commands: Commands, mut gizmos: Gizmos) { + let hand_pose: [Posef; 26] = [ + Posef { position: Vector3f {x: -0.548, y: -0.161, z: -0.137}, orientation: Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}}, //palm + Posef { position: Vector3f {x: -0.548, y: 0.161, z: -0.137}, orientation: Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}}, + Posef { position: Vector3f {x: -0.529, y: -0.198, z: -0.126}, orientation: Quaternionf {x: -0.744, y: -0.530, z: 0.156,w: -0.376}}, + Posef { position: Vector3f {x: -0.533, y: -0.175, z: -0.090}, orientation: Quaternionf {x: -0.786, y: -0.550, z: 0.126,w: -0.254}}, + Posef { position: Vector3f {x: -0.544, y: -0.158, z: -0.069}, orientation: Quaternionf {x: -0.729, y: -0.564, z: 0.027,w: -0.387}}, + Posef { position: Vector3f {x: -0.557, y: -0.150, z: -0.065}, orientation: Quaternionf {x: -0.585, y: -0.548, z: -0.140,w: -0.582}}, + Posef { position: Vector3f {x: -0.521, y: -0.182, z: -0.136}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317,w: -0.376}}, + Posef { position: Vector3f {x: -0.550, y: -0.135, z: -0.102}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317,w: -0.376}}, + Posef { position: Vector3f {x: -0.571, y: -0.112, z: -0.082}, orientation: Quaternionf {x: -0.244, y: -0.843, z: 0.256,w: -0.404}}, + Posef { position: Vector3f {x: -0.585, y: -0.102, z: -0.070}, orientation: Quaternionf {x: -0.200, y: -0.866, z: 0.165,w: -0.428}}, + Posef { position: Vector3f {x: -0.593, y: -0.098, z: -0.064}, orientation: Quaternionf {x: -0.172, y: -0.874, z: 0.110,w: -0.440}}, + Posef { position: Vector3f {x: -0.527, y: -0.178, z: -0.144}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370,w: -0.401}}, + Posef { position: Vector3f {x: -0.559, y: -0.132, z: -0.119}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370,w: -0.401}}, + Posef { position: Vector3f {x: -0.582, y: -0.101, z: -0.104}, orientation: Quaternionf {x: -0.175, y: -0.809, z: 0.371,w: -0.420}}, + Posef { position: Vector3f {x: -0.599, y: -0.089, z: -0.092}, orientation: Quaternionf {x: -0.109, y: -0.856, z: 0.245,w: -0.443}}, + Posef { position: Vector3f {x: -0.608, y: -0.084, z: -0.086}, orientation: Quaternionf {x: -0.075, y: -0.871, z: 0.180,w: -0.450}}, + Posef { position: Vector3f {x: -0.535, y: -0.178, z: -0.152}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408,w: -0.445}}, + Posef { position: Vector3f {x: -0.568, y: -0.136, z: -0.137}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408,w: -0.445}}, + Posef { position: Vector3f {x: -0.590, y: -0.106, z: -0.130}, orientation: Quaternionf {x: -0.131, y: -0.762, z: 0.432,w: -0.464}}, + Posef { position: Vector3f {x: -0.607, y: -0.092, z: -0.122}, orientation: Quaternionf {x: -0.071, y: -0.810, z: 0.332,w: -0.477}}, + Posef { position: Vector3f {x: -0.617, y: -0.086, z: -0.117}, orientation: Quaternionf {x: -0.029, y: -0.836, z: 0.260,w: -0.482}}, + Posef { position: Vector3f {x: -0.544, y: -0.183, z: -0.159}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481,w: -0.452}}, + Posef { position: Vector3f {x: -0.576, y: -0.143, z: -0.152}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481,w: -0.452}}, + Posef { position: Vector3f {x: -0.594, y: -0.119, z: -0.154}, orientation: Quaternionf {x: -0.061, y: -0.684, z: 0.534,w: -0.493}}, + Posef { position: Vector3f {x: -0.607, y: -0.108, z: -0.152}, orientation: Quaternionf {x: 0.002, y: -0.745, z: 0.444,w: -0.498}}, + Posef { position: Vector3f {x: -0.616, y: -0.102, z: -0.150}, orientation: Quaternionf {x: 0.045, y: -0.780, z: 0.378,w: -0.496}}, + ]; + //cursed wrist math + let wrist_dist = Vec3{ x: 0.01, y: -0.05, z: 0.0 }; + let huh = Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}.to_quat(); + let why: Vec3 = huh.mul_vec3(wrist_dist); + + let offset = Vec3 { x: 0.548, y: 1.0, z: 0.137 }; + let palm = hand_pose[HandJoint::PALM]; + gizmos.sphere(palm.position.to_vec3() + offset, palm.orientation.to_quat(), 0.01, Color::WHITE); + let wrist = hand_pose[HandJoint::WRIST]; + gizmos.sphere(palm.position.to_vec3() + offset + why, palm.orientation.to_quat(), 0.01, Color::GRAY); + let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; + gizmos.sphere(thumb_meta.position.to_vec3() + offset, thumb_meta.orientation.to_quat(), 0.01, Color::RED); + let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; + gizmos.sphere(thumb_prox.position.to_vec3() + offset, thumb_prox.orientation.to_quat(), 0.008, Color::RED); + let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; + gizmos.sphere(thumb_dist.position.to_vec3() + offset, thumb_dist.orientation.to_quat(), 0.006, Color::RED); + let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; + gizmos.sphere(thumb_tip.position.to_vec3() + offset, thumb_tip.orientation.to_quat(), 0.004, Color::RED); + + let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; + gizmos.sphere(index_meta.position.to_vec3() + offset, index_meta.orientation.to_quat(), 0.01, Color::ORANGE); + let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; + gizmos.sphere(index_prox.position.to_vec3() + offset, index_prox.orientation.to_quat(), 0.008, Color::ORANGE); + let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; + gizmos.sphere(index_inter.position.to_vec3() + offset, index_inter.orientation.to_quat(), 0.006, Color::ORANGE); + let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; + gizmos.sphere(index_dist.position.to_vec3() + offset, index_dist.orientation.to_quat(), 0.004, Color::ORANGE); + let index_tip = hand_pose[HandJoint::INDEX_TIP]; + gizmos.sphere(index_tip.position.to_vec3() + offset, index_tip.orientation.to_quat(), 0.002, Color::ORANGE); + + let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; + gizmos.sphere(middle_meta.position.to_vec3() + offset, middle_meta.orientation.to_quat(), 0.01, Color::YELLOW); + let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; + gizmos.sphere(middle_prox.position.to_vec3() + offset, middle_prox.orientation.to_quat(), 0.008, Color::YELLOW); + let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; + gizmos.sphere(middle_inter.position.to_vec3() + offset, middle_inter.orientation.to_quat(), 0.006, Color::YELLOW); + let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; + gizmos.sphere(middle_dist.position.to_vec3() + offset, middle_dist.orientation.to_quat(), 0.004, Color::YELLOW); + let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; + gizmos.sphere(middle_tip.position.to_vec3() + offset, middle_tip.orientation.to_quat(), 0.002, Color::YELLOW); + + let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; + gizmos.sphere(ring_meta.position.to_vec3() + offset, ring_meta.orientation.to_quat(), 0.01, Color::GREEN); + let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; + gizmos.sphere(ring_prox.position.to_vec3() + offset, ring_prox.orientation.to_quat(), 0.008, Color::GREEN); + let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; + gizmos.sphere(ring_inter.position.to_vec3() + offset, ring_inter.orientation.to_quat(), 0.006, Color::GREEN); + let ring_dist = hand_pose[HandJoint::RING_DISTAL]; + gizmos.sphere(ring_dist.position.to_vec3() + offset, ring_dist.orientation.to_quat(), 0.004, Color::GREEN); + let ring_tip = hand_pose[HandJoint::RING_TIP]; + gizmos.sphere(ring_tip.position.to_vec3() + offset, ring_tip.orientation.to_quat(), 0.002, Color::GREEN); + + let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; + gizmos.sphere(little_meta.position.to_vec3() + offset, little_meta.orientation.to_quat(), 0.01, Color::BLUE); + let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; + gizmos.sphere(little_prox.position.to_vec3() + offset, little_prox.orientation.to_quat(), 0.008, Color::BLUE); + let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; + gizmos.sphere(little_inter.position.to_vec3() + offset, little_inter.orientation.to_quat(), 0.006, Color::BLUE); + let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; + gizmos.sphere(little_dist.position.to_vec3() + offset, little_dist.orientation.to_quat(), 0.004, Color::BLUE); + let little_tip = hand_pose[HandJoint::LITTLE_TIP]; + gizmos.sphere(little_tip.position.to_vec3() + offset, little_tip.orientation.to_quat(), 0.002, Color::BLUE); + + + + + + +} + + fn spawn_controllers_example(mut commands: Commands) { //left hand commands.spawn(( diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs index f8a70ac..81f5936 100644 --- a/src/xr_input/hand.rs +++ b/src/xr_input/hand.rs @@ -1015,7 +1015,7 @@ pub fn update_hand_skeletons( match input_source { Some(res) => match *res { HandInputSource::Emulated => { - info!("hand input source is emulated"); + // info!("hand input source is emulated"); match hand_states_option { Some(hands) => { let left_hand_transform = left_controller_query From 23e9d1189f50b3203f8ae5c9bd858888191c8592 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 3 Oct 2023 14:28:33 -0400 Subject: [PATCH 55/66] well this is insane but it tracks the hand now --- examples/xr.rs | 152 ++++++++++++++++++++++++++++--------------------- 1 file changed, 88 insertions(+), 64 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 79e4727..fdd91b1 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -82,103 +82,127 @@ fn setup( },)); } -fn spawn_skeleton(mut commands: Commands, mut gizmos: Gizmos) { - let hand_pose: [Posef; 26] = [ - Posef { position: Vector3f {x: -0.548, y: -0.161, z: -0.137}, orientation: Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}}, //palm - Posef { position: Vector3f {x: -0.548, y: 0.161, z: -0.137}, orientation: Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}}, - Posef { position: Vector3f {x: -0.529, y: -0.198, z: -0.126}, orientation: Quaternionf {x: -0.744, y: -0.530, z: 0.156,w: -0.376}}, - Posef { position: Vector3f {x: -0.533, y: -0.175, z: -0.090}, orientation: Quaternionf {x: -0.786, y: -0.550, z: 0.126,w: -0.254}}, - Posef { position: Vector3f {x: -0.544, y: -0.158, z: -0.069}, orientation: Quaternionf {x: -0.729, y: -0.564, z: 0.027,w: -0.387}}, - Posef { position: Vector3f {x: -0.557, y: -0.150, z: -0.065}, orientation: Quaternionf {x: -0.585, y: -0.548, z: -0.140,w: -0.582}}, - Posef { position: Vector3f {x: -0.521, y: -0.182, z: -0.136}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317,w: -0.376}}, - Posef { position: Vector3f {x: -0.550, y: -0.135, z: -0.102}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317,w: -0.376}}, - Posef { position: Vector3f {x: -0.571, y: -0.112, z: -0.082}, orientation: Quaternionf {x: -0.244, y: -0.843, z: 0.256,w: -0.404}}, - Posef { position: Vector3f {x: -0.585, y: -0.102, z: -0.070}, orientation: Quaternionf {x: -0.200, y: -0.866, z: 0.165,w: -0.428}}, - Posef { position: Vector3f {x: -0.593, y: -0.098, z: -0.064}, orientation: Quaternionf {x: -0.172, y: -0.874, z: 0.110,w: -0.440}}, - Posef { position: Vector3f {x: -0.527, y: -0.178, z: -0.144}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370,w: -0.401}}, - Posef { position: Vector3f {x: -0.559, y: -0.132, z: -0.119}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370,w: -0.401}}, - Posef { position: Vector3f {x: -0.582, y: -0.101, z: -0.104}, orientation: Quaternionf {x: -0.175, y: -0.809, z: 0.371,w: -0.420}}, - Posef { position: Vector3f {x: -0.599, y: -0.089, z: -0.092}, orientation: Quaternionf {x: -0.109, y: -0.856, z: 0.245,w: -0.443}}, - Posef { position: Vector3f {x: -0.608, y: -0.084, z: -0.086}, orientation: Quaternionf {x: -0.075, y: -0.871, z: 0.180,w: -0.450}}, - Posef { position: Vector3f {x: -0.535, y: -0.178, z: -0.152}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408,w: -0.445}}, - Posef { position: Vector3f {x: -0.568, y: -0.136, z: -0.137}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408,w: -0.445}}, - Posef { position: Vector3f {x: -0.590, y: -0.106, z: -0.130}, orientation: Quaternionf {x: -0.131, y: -0.762, z: 0.432,w: -0.464}}, - Posef { position: Vector3f {x: -0.607, y: -0.092, z: -0.122}, orientation: Quaternionf {x: -0.071, y: -0.810, z: 0.332,w: -0.477}}, - Posef { position: Vector3f {x: -0.617, y: -0.086, z: -0.117}, orientation: Quaternionf {x: -0.029, y: -0.836, z: 0.260,w: -0.482}}, - Posef { position: Vector3f {x: -0.544, y: -0.183, z: -0.159}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481,w: -0.452}}, - Posef { position: Vector3f {x: -0.576, y: -0.143, z: -0.152}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481,w: -0.452}}, - Posef { position: Vector3f {x: -0.594, y: -0.119, z: -0.154}, orientation: Quaternionf {x: -0.061, y: -0.684, z: 0.534,w: -0.493}}, - Posef { position: Vector3f {x: -0.607, y: -0.108, z: -0.152}, orientation: Quaternionf {x: 0.002, y: -0.745, z: 0.444,w: -0.498}}, - Posef { position: Vector3f {x: -0.616, y: -0.102, z: -0.150}, orientation: Quaternionf {x: 0.045, y: -0.780, z: 0.378,w: -0.496}}, - ]; - //cursed wrist math - let wrist_dist = Vec3{ x: 0.01, y: -0.05, z: 0.0 }; - let huh = Quaternionf {x: -0.267,y: 0.849, z: 0.204,w: 0.407}.to_quat(); - let why: Vec3 = huh.mul_vec3(wrist_dist); +fn draw_skeleton_hand(mut commands: Commands, + mut gizmos: Gizmos, + right_controller_query: Query<( + &GlobalTransform, + With, +)>, ) { + //draw debug for controller grip center to match palm to + let right_transform = right_controller_query.get_single().unwrap().0; + let right_translation = right_transform.compute_transform().translation; + let right_quat = right_transform.compute_transform().rotation; + gizmos.sphere(right_translation, Quat::IDENTITY, 0.01, Color::PINK); + //we need to flip this i dont know why + let flip = Quat::from_rotation_x(PI); + let flap = Quat::from_rotation_z(-45.0*(PI/180.0)); + let controller_forward = right_quat.mul_quat(flip).mul_vec3(Vec3::Y); + let controller_backward = right_quat.mul_quat(flip).mul_quat(flap); + gizmos.ray(right_translation, controller_forward, Color::PINK); - let offset = Vec3 { x: 0.548, y: 1.0, z: 0.137 }; + let hand_pose: [Posef; 26] = [ + Posef { position: Vector3f {x: -0.548, y: -0.161, z: -0.137}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, //palm + Posef { position: Vector3f {x: -0.548, y: 0.161, z: -0.137}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, + Posef { position: Vector3f {x: -0.529, y: -0.198, z: -0.126}, orientation: Quaternionf {x: -0.744, y: -0.530, z: 0.156, w: -0.376}}, + Posef { position: Vector3f {x: -0.533, y: -0.175, z: -0.090}, orientation: Quaternionf {x: -0.786, y: -0.550, z: 0.126, w: -0.254}}, + Posef { position: Vector3f {x: -0.544, y: -0.158, z: -0.069}, orientation: Quaternionf {x: -0.729, y: -0.564, z: 0.027, w: -0.387}}, + Posef { position: Vector3f {x: -0.557, y: -0.150, z: -0.065}, orientation: Quaternionf {x: -0.585, y: -0.548, z: -0.140,w: -0.582}}, + Posef { position: Vector3f {x: -0.521, y: -0.182, z: -0.136}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317, w: -0.376}}, + Posef { position: Vector3f {x: -0.550, y: -0.135, z: -0.102}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317, w: -0.376}}, + Posef { position: Vector3f {x: -0.571, y: -0.112, z: -0.082}, orientation: Quaternionf {x: -0.244, y: -0.843, z: 0.256, w: -0.404}}, + Posef { position: Vector3f {x: -0.585, y: -0.102, z: -0.070}, orientation: Quaternionf {x: -0.200, y: -0.866, z: 0.165, w: -0.428}}, + Posef { position: Vector3f {x: -0.593, y: -0.098, z: -0.064}, orientation: Quaternionf {x: -0.172, y: -0.874, z: 0.110, w: -0.440}}, + Posef { position: Vector3f {x: -0.527, y: -0.178, z: -0.144}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370, w: -0.401}}, + Posef { position: Vector3f {x: -0.559, y: -0.132, z: -0.119}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370, w: -0.401}}, + Posef { position: Vector3f {x: -0.582, y: -0.101, z: -0.104}, orientation: Quaternionf {x: -0.175, y: -0.809, z: 0.371, w: -0.420}}, + Posef { position: Vector3f {x: -0.599, y: -0.089, z: -0.092}, orientation: Quaternionf {x: -0.109, y: -0.856, z: 0.245, w: -0.443}}, + Posef { position: Vector3f {x: -0.608, y: -0.084, z: -0.086}, orientation: Quaternionf {x: -0.075, y: -0.871, z: 0.180, w: -0.450}}, + Posef { position: Vector3f {x: -0.535, y: -0.178, z: -0.152}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408, w: -0.445}}, + Posef { position: Vector3f {x: -0.568, y: -0.136, z: -0.137}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408, w: -0.445}}, + Posef { position: Vector3f {x: -0.590, y: -0.106, z: -0.130}, orientation: Quaternionf {x: -0.131, y: -0.762, z: 0.432, w: -0.464}}, + Posef { position: Vector3f {x: -0.607, y: -0.092, z: -0.122}, orientation: Quaternionf {x: -0.071, y: -0.810, z: 0.332, w: -0.477}}, + Posef { position: Vector3f {x: -0.617, y: -0.086, z: -0.117}, orientation: Quaternionf {x: -0.029, y: -0.836, z: 0.260, w: -0.482}}, + Posef { position: Vector3f {x: -0.544, y: -0.183, z: -0.159}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481, w: -0.452}}, + Posef { position: Vector3f {x: -0.576, y: -0.143, z: -0.152}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481, w: -0.452}}, + Posef { position: Vector3f {x: -0.594, y: -0.119, z: -0.154}, orientation: Quaternionf {x: -0.061, y: -0.684, z: 0.534, w: -0.493}}, + Posef { position: Vector3f {x: -0.607, y: -0.108, z: -0.152}, orientation: Quaternionf {x: 0.002, y: -0.745, z: 0.444, w: -0.498}}, + Posef { position: Vector3f {x: -0.616, y: -0.102, z: -0.150}, orientation: Quaternionf {x: 0.045, y: -0.780, z: 0.378, w: -0.496}}, + ]; + + //cursed wrist math + let wrist_dist = Vec3{ x: -0.01, y: -0.040, z: -0.015}; + + //cursed offset + let palm_negation = Vec3 { x: 0.548, y: 0.161, z: 0.137 }; + let offset = right_translation; + + //old stuff dont touch for now let palm = hand_pose[HandJoint::PALM]; - gizmos.sphere(palm.position.to_vec3() + offset, palm.orientation.to_quat(), 0.01, Color::WHITE); - let wrist = hand_pose[HandJoint::WRIST]; - gizmos.sphere(palm.position.to_vec3() + offset + why, palm.orientation.to_quat(), 0.01, Color::GRAY); + gizmos.sphere(palm.position.to_vec3() + offset + palm_negation, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); + + let rotated_wfp = palm.position.to_vec3() + right_quat.mul_quat(flip).mul_vec3(wrist_dist); + gizmos.sphere(offset + palm_negation + rotated_wfp, palm.orientation.to_quat(), 0.01, Color::GRAY); + + let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; - gizmos.sphere(thumb_meta.position.to_vec3() + offset, thumb_meta.orientation.to_quat(), 0.01, Color::RED); + draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, palm_negation, offset); + let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; - gizmos.sphere(thumb_prox.position.to_vec3() + offset, thumb_prox.orientation.to_quat(), 0.008, Color::RED); + draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, palm_negation, offset); let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; - gizmos.sphere(thumb_dist.position.to_vec3() + offset, thumb_dist.orientation.to_quat(), 0.006, Color::RED); + draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, palm_negation, offset); let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; - gizmos.sphere(thumb_tip.position.to_vec3() + offset, thumb_tip.orientation.to_quat(), 0.004, Color::RED); + draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, palm_negation, offset); let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; - gizmos.sphere(index_meta.position.to_vec3() + offset, index_meta.orientation.to_quat(), 0.01, Color::ORANGE); + draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, palm_negation, offset); let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; - gizmos.sphere(index_prox.position.to_vec3() + offset, index_prox.orientation.to_quat(), 0.008, Color::ORANGE); + draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, palm_negation, offset); let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; - gizmos.sphere(index_inter.position.to_vec3() + offset, index_inter.orientation.to_quat(), 0.006, Color::ORANGE); + draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, palm_negation, offset); let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; - gizmos.sphere(index_dist.position.to_vec3() + offset, index_dist.orientation.to_quat(), 0.004, Color::ORANGE); + draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, palm_negation, offset); let index_tip = hand_pose[HandJoint::INDEX_TIP]; - gizmos.sphere(index_tip.position.to_vec3() + offset, index_tip.orientation.to_quat(), 0.002, Color::ORANGE); + draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, palm_negation, offset); let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; - gizmos.sphere(middle_meta.position.to_vec3() + offset, middle_meta.orientation.to_quat(), 0.01, Color::YELLOW); + draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, palm_negation, offset); let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; - gizmos.sphere(middle_prox.position.to_vec3() + offset, middle_prox.orientation.to_quat(), 0.008, Color::YELLOW); + draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, palm_negation, offset); let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; - gizmos.sphere(middle_inter.position.to_vec3() + offset, middle_inter.orientation.to_quat(), 0.006, Color::YELLOW); + draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, palm_negation, offset); let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; - gizmos.sphere(middle_dist.position.to_vec3() + offset, middle_dist.orientation.to_quat(), 0.004, Color::YELLOW); + draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, palm_negation, offset); let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; - gizmos.sphere(middle_tip.position.to_vec3() + offset, middle_tip.orientation.to_quat(), 0.002, Color::YELLOW); + draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, palm_negation, offset); let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; - gizmos.sphere(ring_meta.position.to_vec3() + offset, ring_meta.orientation.to_quat(), 0.01, Color::GREEN); + draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, palm_negation, offset); let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; - gizmos.sphere(ring_prox.position.to_vec3() + offset, ring_prox.orientation.to_quat(), 0.008, Color::GREEN); + draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, palm_negation, offset); let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; - gizmos.sphere(ring_inter.position.to_vec3() + offset, ring_inter.orientation.to_quat(), 0.006, Color::GREEN); + draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, palm_negation, offset); let ring_dist = hand_pose[HandJoint::RING_DISTAL]; - gizmos.sphere(ring_dist.position.to_vec3() + offset, ring_dist.orientation.to_quat(), 0.004, Color::GREEN); + draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, palm_negation, offset); let ring_tip = hand_pose[HandJoint::RING_TIP]; - gizmos.sphere(ring_tip.position.to_vec3() + offset, ring_tip.orientation.to_quat(), 0.002, Color::GREEN); + draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, palm_negation, offset); let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; - gizmos.sphere(little_meta.position.to_vec3() + offset, little_meta.orientation.to_quat(), 0.01, Color::BLUE); + draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, palm_negation, offset); let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; - gizmos.sphere(little_prox.position.to_vec3() + offset, little_prox.orientation.to_quat(), 0.008, Color::BLUE); + draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, palm_negation, offset); let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; - gizmos.sphere(little_inter.position.to_vec3() + offset, little_inter.orientation.to_quat(), 0.006, Color::BLUE); + draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, palm_negation, offset); let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; - gizmos.sphere(little_dist.position.to_vec3() + offset, little_dist.orientation.to_quat(), 0.004, Color::BLUE); + draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, palm_negation, offset); let little_tip = hand_pose[HandJoint::LITTLE_TIP]; - gizmos.sphere(little_tip.position.to_vec3() + offset, little_tip.orientation.to_quat(), 0.002, Color::BLUE); - - - + draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, palm_negation, offset); +} +fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, palm_negation: Vec3, offset: Vec3) { + gizmos.sphere(controller_backwards.mul_vec3(joint_pos + palm_negation) + offset, joint_rot, radius, color); } From 69d5b13950093c9236162245e4b05fd14735ac38 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 3 Oct 2023 15:03:34 -0400 Subject: [PATCH 56/66] normalized hand poses to palm --- examples/xr.rs | 157 +++++++++++++++++++++++++++++++------------------ 1 file changed, 99 insertions(+), 58 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index fdd91b1..783149d 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -92,117 +92,158 @@ fn draw_skeleton_hand(mut commands: Commands, let right_transform = right_controller_query.get_single().unwrap().0; let right_translation = right_transform.compute_transform().translation; let right_quat = right_transform.compute_transform().rotation; - gizmos.sphere(right_translation, Quat::IDENTITY, 0.01, Color::PINK); //we need to flip this i dont know why let flip = Quat::from_rotation_x(PI); let flap = Quat::from_rotation_z(-45.0*(PI/180.0)); - let controller_forward = right_quat.mul_quat(flip).mul_vec3(Vec3::Y); let controller_backward = right_quat.mul_quat(flip).mul_quat(flap); - gizmos.ray(right_translation, controller_forward, Color::PINK); let hand_pose: [Posef; 26] = [ - Posef { position: Vector3f {x: -0.548, y: -0.161, z: -0.137}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, //palm - Posef { position: Vector3f {x: -0.548, y: 0.161, z: -0.137}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, - Posef { position: Vector3f {x: -0.529, y: -0.198, z: -0.126}, orientation: Quaternionf {x: -0.744, y: -0.530, z: 0.156, w: -0.376}}, - Posef { position: Vector3f {x: -0.533, y: -0.175, z: -0.090}, orientation: Quaternionf {x: -0.786, y: -0.550, z: 0.126, w: -0.254}}, - Posef { position: Vector3f {x: -0.544, y: -0.158, z: -0.069}, orientation: Quaternionf {x: -0.729, y: -0.564, z: 0.027, w: -0.387}}, - Posef { position: Vector3f {x: -0.557, y: -0.150, z: -0.065}, orientation: Quaternionf {x: -0.585, y: -0.548, z: -0.140,w: -0.582}}, - Posef { position: Vector3f {x: -0.521, y: -0.182, z: -0.136}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317, w: -0.376}}, - Posef { position: Vector3f {x: -0.550, y: -0.135, z: -0.102}, orientation: Quaternionf {x: -0.277, y: -0.826, z: 0.317, w: -0.376}}, - Posef { position: Vector3f {x: -0.571, y: -0.112, z: -0.082}, orientation: Quaternionf {x: -0.244, y: -0.843, z: 0.256, w: -0.404}}, - Posef { position: Vector3f {x: -0.585, y: -0.102, z: -0.070}, orientation: Quaternionf {x: -0.200, y: -0.866, z: 0.165, w: -0.428}}, - Posef { position: Vector3f {x: -0.593, y: -0.098, z: -0.064}, orientation: Quaternionf {x: -0.172, y: -0.874, z: 0.110, w: -0.440}}, - Posef { position: Vector3f {x: -0.527, y: -0.178, z: -0.144}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370, w: -0.401}}, - Posef { position: Vector3f {x: -0.559, y: -0.132, z: -0.119}, orientation: Quaternionf {x: -0.185, y: -0.817, z: 0.370, w: -0.401}}, - Posef { position: Vector3f {x: -0.582, y: -0.101, z: -0.104}, orientation: Quaternionf {x: -0.175, y: -0.809, z: 0.371, w: -0.420}}, - Posef { position: Vector3f {x: -0.599, y: -0.089, z: -0.092}, orientation: Quaternionf {x: -0.109, y: -0.856, z: 0.245, w: -0.443}}, - Posef { position: Vector3f {x: -0.608, y: -0.084, z: -0.086}, orientation: Quaternionf {x: -0.075, y: -0.871, z: 0.180, w: -0.450}}, - Posef { position: Vector3f {x: -0.535, y: -0.178, z: -0.152}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408, w: -0.445}}, - Posef { position: Vector3f {x: -0.568, y: -0.136, z: -0.137}, orientation: Quaternionf {x: -0.132, y: -0.786, z: 0.408, w: -0.445}}, - Posef { position: Vector3f {x: -0.590, y: -0.106, z: -0.130}, orientation: Quaternionf {x: -0.131, y: -0.762, z: 0.432, w: -0.464}}, - Posef { position: Vector3f {x: -0.607, y: -0.092, z: -0.122}, orientation: Quaternionf {x: -0.071, y: -0.810, z: 0.332, w: -0.477}}, - Posef { position: Vector3f {x: -0.617, y: -0.086, z: -0.117}, orientation: Quaternionf {x: -0.029, y: -0.836, z: 0.260, w: -0.482}}, - Posef { position: Vector3f {x: -0.544, y: -0.183, z: -0.159}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481, w: -0.452}}, - Posef { position: Vector3f {x: -0.576, y: -0.143, z: -0.152}, orientation: Quaternionf {x: -0.060, y: -0.749, z: 0.481, w: -0.452}}, - Posef { position: Vector3f {x: -0.594, y: -0.119, z: -0.154}, orientation: Quaternionf {x: -0.061, y: -0.684, z: 0.534, w: -0.493}}, - Posef { position: Vector3f {x: -0.607, y: -0.108, z: -0.152}, orientation: Quaternionf {x: 0.002, y: -0.745, z: 0.444, w: -0.498}}, - Posef { position: Vector3f {x: -0.616, y: -0.102, z: -0.150}, orientation: Quaternionf {x: 0.045, y: -0.780, z: 0.378, w: -0.496}}, + 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.0, y: 0.32, z: 0.0}, 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}}, ]; + //log_hand(hand_pose); + + //cursed wrist math let wrist_dist = Vec3{ x: -0.01, y: -0.040, z: -0.015}; //cursed offset - let palm_negation = Vec3 { x: 0.548, y: 0.161, z: 0.137 }; let offset = right_translation; //old stuff dont touch for now let palm = hand_pose[HandJoint::PALM]; - gizmos.sphere(palm.position.to_vec3() + offset + palm_negation, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); + gizmos.sphere(palm.position.to_vec3() + offset, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); let rotated_wfp = palm.position.to_vec3() + right_quat.mul_quat(flip).mul_vec3(wrist_dist); - gizmos.sphere(offset + palm_negation + rotated_wfp, palm.orientation.to_quat(), 0.01, Color::GRAY); + gizmos.sphere(offset + rotated_wfp, palm.orientation.to_quat(), 0.01, Color::GRAY); let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; - draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, offset); let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; - draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, offset); let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; - draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, offset); let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; - draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, offset); let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; - draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, offset); let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; - draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, offset); let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; - draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, offset); let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; - draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, offset); let index_tip = hand_pose[HandJoint::INDEX_TIP]; - draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, offset); let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; - draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, offset); let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; - draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, offset); let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; - draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, offset); let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; - draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, offset); let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; - draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, offset); let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; - draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, offset); let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; - draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, offset); let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; - draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, offset); let ring_dist = hand_pose[HandJoint::RING_DISTAL]; - draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, offset); let ring_tip = hand_pose[HandJoint::RING_TIP]; - draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, offset); let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; - draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, offset); let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; - draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, offset); let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; - draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, offset); let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; - draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, offset); let little_tip = hand_pose[HandJoint::LITTLE_TIP]; - draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, palm_negation, offset); + draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, offset); + + } -fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, palm_negation: Vec3, offset: Vec3) { - gizmos.sphere(controller_backwards.mul_vec3(joint_pos + palm_negation) + offset, joint_rot, radius, color); +fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, offset: Vec3) { + gizmos.sphere(controller_backwards.mul_vec3(joint_pos) + offset, joint_rot, radius, color); +} + +fn log_hand(hand_pose: [Posef; 26]) { + let palm_vec = hand_pose[HandJoint::PALM].position.to_vec3(); + info!("palm: {}", hand_pose[HandJoint::PALM].position.to_vec3() - palm_vec); + info!("wrist: {}", hand_pose[HandJoint::WRIST].position.to_vec3() - palm_vec); + + info!("tm: {}", hand_pose[HandJoint::THUMB_METACARPAL].position.to_vec3() - palm_vec); + info!("tp: {}", hand_pose[HandJoint::THUMB_PROXIMAL].position.to_vec3() - palm_vec); + info!("td: {}", hand_pose[HandJoint::THUMB_DISTAL].position.to_vec3() - palm_vec); + info!("tt: {}", hand_pose[HandJoint::THUMB_TIP].position.to_vec3() - palm_vec); + + info!("im: {}", hand_pose[HandJoint::INDEX_METACARPAL].position.to_vec3() - palm_vec); + info!("ip: {}", hand_pose[HandJoint::INDEX_PROXIMAL].position.to_vec3() - palm_vec); + info!("ii: {}", hand_pose[HandJoint::INDEX_INTERMEDIATE].position.to_vec3() - palm_vec); + info!("id: {}", hand_pose[HandJoint::INDEX_DISTAL].position.to_vec3() - palm_vec); + info!("it: {}", hand_pose[HandJoint::INDEX_TIP].position.to_vec3() - palm_vec); + + info!("mm: {}", hand_pose[HandJoint::MIDDLE_METACARPAL].position.to_vec3() - palm_vec); + info!("mp: {}", hand_pose[HandJoint::MIDDLE_PROXIMAL].position.to_vec3() - palm_vec); + info!("mi: {}", hand_pose[HandJoint::MIDDLE_INTERMEDIATE].position.to_vec3() - palm_vec); + info!("md: {}", hand_pose[HandJoint::MIDDLE_DISTAL].position.to_vec3() - palm_vec); + info!("mt: {}", hand_pose[HandJoint::MIDDLE_TIP].position.to_vec3() - palm_vec); + + info!("rm: {}", hand_pose[HandJoint::RING_METACARPAL].position.to_vec3() - palm_vec); + info!("rp: {}", hand_pose[HandJoint::RING_PROXIMAL].position.to_vec3() - palm_vec); + info!("ri: {}", hand_pose[HandJoint::RING_INTERMEDIATE].position.to_vec3() - palm_vec); + info!("rd: {}", hand_pose[HandJoint::RING_DISTAL].position.to_vec3() - palm_vec); + info!("rt: {}", hand_pose[HandJoint::RING_TIP].position.to_vec3() - palm_vec); + + info!("lm: {}", hand_pose[HandJoint::LITTLE_METACARPAL].position.to_vec3() - palm_vec); + info!("lp: {}", hand_pose[HandJoint::LITTLE_PROXIMAL].position.to_vec3() - palm_vec); + info!("li: {}", hand_pose[HandJoint::LITTLE_INTERMEDIATE].position.to_vec3() - palm_vec); + info!("ld: {}", hand_pose[HandJoint::LITTLE_DISTAL].position.to_vec3() - palm_vec); + info!("lt: {}", hand_pose[HandJoint::LITTLE_TIP].position.to_vec3() - palm_vec); } From 0b43977e0781350463e9930ff40e4e59422c4fa1 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 3 Oct 2023 15:47:18 -0400 Subject: [PATCH 57/66] both hands work --- examples/xr.rs | 127 +++++++++++++++++++++++++++++++------------------ 1 file changed, 80 insertions(+), 47 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 783149d..d14f9c4 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -1,12 +1,22 @@ +<<<<<<< HEAD +======= +use std::f32::consts::PI; +use std::ops::Mul; +>>>>>>> 68cdf19 (both hands work) use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; +<<<<<<< HEAD use bevy_openxr::xr_input::{QuatConv, Vec3Conv}; use bevy_openxr::xr_input::hand::{OpenXrHandInput, HandInputDebugRenderer}; +======= +use bevy_openxr::xr_input::{Vec3Conv, QuatConv, Hand}; +use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; +>>>>>>> 68cdf19 (both hands work) use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, @@ -25,6 +35,11 @@ fn main() { .add_plugins(FrameTimeDiagnosticsPlugin) .add_systems(Startup, setup) .add_systems(Update, proto_locomotion) +<<<<<<< HEAD +======= + .add_systems(Startup, spawn_controllers_example) + .add_systems(Update, draw_skeleton_hands) +>>>>>>> 68cdf19 (both hands work) .insert_resource(PrototypeLocomotionConfig::default()) .add_systems(Startup, spawn_controllers_example) .add_plugins(OpenXrHandInput) @@ -82,24 +97,39 @@ fn setup( },)); } -fn draw_skeleton_hand(mut commands: Commands, +fn draw_skeleton_hands(mut commands: Commands, mut gizmos: Gizmos, right_controller_query: Query<( &GlobalTransform, With, -)>, ) { +)>, +left_controller_query: Query<( + &GlobalTransform, + With, +)>,) { + + let left_hand_transform = left_controller_query.get_single().unwrap().0.compute_transform(); + draw_hand(&mut gizmos, left_hand_transform, Hand::Left); + let right_hand_transform = right_controller_query.get_single().unwrap().0.compute_transform(); + draw_hand(&mut gizmos, right_hand_transform, Hand::Right) + + + +} + +fn draw_hand(mut gizmos: &mut Gizmos, + controller_transform: Transform, +hand: Hand) { //draw debug for controller grip center to match palm to - let right_transform = right_controller_query.get_single().unwrap().0; - let right_translation = right_transform.compute_transform().translation; - let right_quat = right_transform.compute_transform().rotation; + let hand_translation = controller_transform.translation; + let hand_quat = controller_transform.rotation; //we need to flip this i dont know why let flip = Quat::from_rotation_x(PI); - let flap = Quat::from_rotation_z(-45.0*(PI/180.0)); - let controller_backward = right_quat.mul_quat(flip).mul_quat(flap); + let controller_backward = hand_quat.mul_quat(flip); - let hand_pose: [Posef; 26] = [ + let test: [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.0, y: 0.32, z: 0.0}, orientation: Quaternionf {x: -0.267, y: 0.849, z: 0.204, w: 0.407}}, + 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}}, @@ -131,80 +161,83 @@ fn draw_skeleton_hand(mut commands: Commands, 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}}, ]; + let hand_pose = flip_hand_pose(test.clone(), hand); + //log_hand(hand_pose); - - //cursed wrist math - let wrist_dist = Vec3{ x: -0.01, y: -0.040, z: -0.015}; - - //cursed offset - let offset = right_translation; - - //old stuff dont touch for now let palm = hand_pose[HandJoint::PALM]; - gizmos.sphere(palm.position.to_vec3() + offset, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); + gizmos.sphere(palm.position.to_vec3() + hand_translation, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); - let rotated_wfp = palm.position.to_vec3() + right_quat.mul_quat(flip).mul_vec3(wrist_dist); - gizmos.sphere(offset + rotated_wfp, palm.orientation.to_quat(), 0.01, Color::GRAY); + let wrist = hand_pose[HandJoint::WRIST]; + draw_joint(&mut gizmos, wrist.position.to_vec3(), wrist.orientation.to_quat(), 0.01, Color::GRAY, controller_backward, hand_translation); let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; - draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, offset); + draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, hand_translation); let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; - draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, offset); + draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, hand_translation); let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; - draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, offset); + draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, hand_translation); let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; - draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, offset); + draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, hand_translation); let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; - draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, hand_translation); let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; - draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, hand_translation); let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; - draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, hand_translation); let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; - draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, hand_translation); let index_tip = hand_pose[HandJoint::INDEX_TIP]; - draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, offset); + draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, hand_translation); let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; - draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, hand_translation); let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; - draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, hand_translation); let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; - draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, hand_translation); let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; - draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, hand_translation); let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; - draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, offset); + draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, hand_translation); let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; - draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, hand_translation); let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; - draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, hand_translation); let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; - draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, hand_translation); let ring_dist = hand_pose[HandJoint::RING_DISTAL]; - draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, hand_translation); let ring_tip = hand_pose[HandJoint::RING_TIP]; - draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, offset); + draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, hand_translation); let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; - draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, offset); + draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, hand_translation); let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; - draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, offset); + draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, hand_translation); let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; - draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, offset); + draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, hand_translation); let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; - draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, offset); + draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, hand_translation); let little_tip = hand_pose[HandJoint::LITTLE_TIP]; - draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, offset); - - - + draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, hand_translation); +} +fn flip_hand_pose(hand_pose: [Posef; 26], hand: Hand) -> [Posef; 26] { + let mut new_pose = hand_pose; + match hand { + Hand::Left => { + for pose in new_pose.iter_mut() { + pose.position.x = -pose.position.x; + } + }, + Hand::Right => (), + } + return new_pose; } fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, offset: Vec3) { From d087cc323fe529105641a449807bc82164b44766 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sat, 7 Oct 2023 17:05:47 -0400 Subject: [PATCH 58/66] left hand works too --- examples/xr.rs | 830 +++++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 692 insertions(+), 138 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index d14f9c4..f011a96 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -21,8 +21,9 @@ use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLoc use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, }; +use bevy_openxr::xr_input::{Hand, QuatConv, Vec3Conv}; use bevy_openxr::DefaultXrPlugins; -use openxr::{Posef, Quaternionf, Vector3f, HandJoint}; +use openxr::{HandJoint, Posef, Quaternionf, Vector3f}; fn main() { color_eyre::install().unwrap(); @@ -97,29 +98,294 @@ fn setup( },)); } -fn draw_skeleton_hands(mut commands: Commands, +fn draw_skeleton_hands( + mut commands: Commands, mut gizmos: Gizmos, - right_controller_query: Query<( - &GlobalTransform, - With, -)>, -left_controller_query: Query<( - &GlobalTransform, - With, -)>,) { - - let left_hand_transform = left_controller_query.get_single().unwrap().0.compute_transform(); - draw_hand(&mut gizmos, left_hand_transform, Hand::Left); - let right_hand_transform = right_controller_query.get_single().unwrap().0.compute_transform(); - draw_hand(&mut gizmos, right_hand_transform, Hand::Right) - - - + right_controller_query: Query<(&GlobalTransform, With)>, + left_controller_query: Query<(&GlobalTransform, With)>, +) { + let left_hand_transform = left_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left); + let right_hand_transform = right_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); + draw_hand_bones(&mut gizmos, right_hand_transform, Hand::Right); } -fn draw_hand(mut gizmos: &mut Gizmos, - controller_transform: Transform, -hand: Hand) { +fn pose_array_to_transform_array(hand_pose: [Posef; 26]) -> [Transform; 26] { + let mut result_array: [Transform; 26] = [Transform::default(); 26]; + for (place, data) in result_array.iter_mut().zip(hand_pose.iter()) { + *place = Transform { + translation: data.position.to_vec3(), + rotation: data.orientation.to_quat(), + scale: Vec3::splat(1.0), + } + } + return result_array; +} + +fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: 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, + }; + //get paml 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); + //draw debug rays + gizmos.ray( + hand_translation, + palm_quat.mul_vec3(Vec3::Z * 0.2), + Color::BLUE, + ); + gizmos.ray( + hand_translation, + palm_quat.mul_vec3(Vec3::Y * 0.2), + Color::GREEN, + ); + gizmos.ray( + hand_translation, + palm_quat.mul_vec3(Vec3::X * 0.2), + Color::RED, + ); + //get simulated bones + let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); + //draw controller-palm bone(should be zero length) + let palm = hand_transform_array[HandJoint::PALM]; + gizmos.ray(hand_translation, palm.translation, Color::WHITE); + //draw palm-wrist + let wrist = hand_transform_array[HandJoint::WRIST]; + gizmos.ray( + hand_translation + palm.translation, + palm_quat.mul_vec3(wrist.translation), + Color::GRAY, + ); + + //thumb + //better finger drawing? + 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 color = Color::RED; + let splay = Quat::from_rotation_y(splay_direction * 30.0 * PI / 180.0); + let splay_quat = palm_quat.mul_quat(splay); + for bone in thumb_joints.iter() { + match prior_start { + Some(start) => { + let tp_lrot = Quat::from_rotation_y(splay_direction * 5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } + + //better finger drawing? + 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 color = Color::ORANGE; + 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 tp_lrot = Quat::from_rotation_x(-5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } + + //better finger drawing? + 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 color = Color::YELLOW; + 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 tp_lrot = Quat::from_rotation_x(-5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } + //better finger drawing? + 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 color = Color::GREEN; + 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 tp_lrot = Quat::from_rotation_x(-5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } + + //better finger drawing? + 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 color = Color::BLUE; + 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 tp_lrot = Quat::from_rotation_x(-5.0 * 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); + gizmos.ray(tp_start, tp_vector, color); + prior_start = Some(tp_start); + prior_quat = Some(tp_quat); + prior_vector = Some(tp_vector); + } + 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); + gizmos.ray(tm_start, tm_vector, color); + prior_start = Some(tm_start); + prior_quat = Some(splay_quat); + prior_vector = Some(tm_vector); + } + } + } +} + +fn draw_hand(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: Hand) { //draw debug for controller grip center to match palm to let hand_translation = controller_transform.translation; let hand_quat = controller_transform.rotation; @@ -127,104 +393,280 @@ hand: Hand) { let flip = Quat::from_rotation_x(PI); let controller_backward = hand_quat.mul_quat(flip); - let test: [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}}, + let test_hand_pose = get_test_hand_pose_array(); - 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}}, + let hand_pose = flip_hand_pose(test_hand_pose.clone(), hand); - 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}}, + // let hand_transform_array: [Transform; 26] = pose_array_to_transform_array(hand_pose); + let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); - 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}}, + let palm = hand_transform_array[HandJoint::PALM]; + gizmos.sphere( + palm.translation + hand_translation, + palm.rotation.mul_quat(controller_backward), + 0.01, + Color::WHITE, + ); - 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}}, + let wrist = hand_transform_array[HandJoint::WRIST]; + draw_joint( + &mut gizmos, + wrist.translation, + wrist.rotation, + 0.01, + Color::GRAY, + controller_backward, + hand_translation, + ); - 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}}, - ]; + let thumb_meta = hand_transform_array[HandJoint::THUMB_METACARPAL]; + draw_joint( + &mut gizmos, + thumb_meta.translation, + thumb_meta.rotation, + 0.01, + Color::RED, + controller_backward, + hand_translation, + ); - let hand_pose = flip_hand_pose(test.clone(), hand); + let thumb_prox = hand_transform_array[HandJoint::THUMB_PROXIMAL]; + draw_joint( + &mut gizmos, + thumb_prox.translation, + thumb_prox.rotation, + 0.008, + Color::RED, + controller_backward, + hand_translation, + ); + let thumb_dist = hand_transform_array[HandJoint::THUMB_DISTAL]; + draw_joint( + &mut gizmos, + thumb_dist.translation, + thumb_dist.rotation, + 0.006, + Color::RED, + controller_backward, + hand_translation, + ); + let thumb_tip = hand_transform_array[HandJoint::THUMB_TIP]; + draw_joint( + &mut gizmos, + thumb_tip.translation, + thumb_tip.rotation, + 0.004, + Color::RED, + controller_backward, + hand_translation, + ); - //log_hand(hand_pose); + let index_meta = hand_transform_array[HandJoint::INDEX_METACARPAL]; + draw_joint( + &mut gizmos, + index_meta.translation, + index_meta.rotation, + 0.01, + Color::ORANGE, + controller_backward, + hand_translation, + ); + let index_prox = hand_transform_array[HandJoint::INDEX_PROXIMAL]; + draw_joint( + &mut gizmos, + index_prox.translation, + index_prox.rotation, + 0.008, + Color::ORANGE, + controller_backward, + hand_translation, + ); - let palm = hand_pose[HandJoint::PALM]; - gizmos.sphere(palm.position.to_vec3() + hand_translation, palm.orientation.to_quat().mul_quat(controller_backward), 0.01, Color::WHITE); + let index_inter = hand_transform_array[HandJoint::INDEX_INTERMEDIATE]; + draw_joint( + &mut gizmos, + index_inter.translation, + index_inter.rotation, + 0.006, + Color::ORANGE, + controller_backward, + hand_translation, + ); - let wrist = hand_pose[HandJoint::WRIST]; - draw_joint(&mut gizmos, wrist.position.to_vec3(), wrist.orientation.to_quat(), 0.01, Color::GRAY, controller_backward, hand_translation); + let index_dist = hand_transform_array[HandJoint::INDEX_DISTAL]; + draw_joint( + &mut gizmos, + index_dist.translation, + index_dist.rotation, + 0.004, + Color::ORANGE, + controller_backward, + hand_translation, + ); + let index_tip = hand_transform_array[HandJoint::INDEX_TIP]; + draw_joint( + &mut gizmos, + index_tip.translation, + index_tip.rotation, + 0.002, + Color::ORANGE, + controller_backward, + hand_translation, + ); - let thumb_meta = hand_pose[HandJoint::THUMB_METACARPAL]; - draw_joint(&mut gizmos, thumb_meta.position.to_vec3(), thumb_meta.orientation.to_quat(), 0.01, Color::RED, controller_backward, hand_translation); + let middle_meta = hand_transform_array[HandJoint::MIDDLE_METACARPAL]; + draw_joint( + &mut gizmos, + middle_meta.translation, + middle_meta.rotation, + 0.01, + Color::YELLOW, + controller_backward, + hand_translation, + ); + let middle_prox = hand_transform_array[HandJoint::MIDDLE_PROXIMAL]; + draw_joint( + &mut gizmos, + middle_prox.translation, + middle_prox.rotation, + 0.008, + Color::YELLOW, + controller_backward, + hand_translation, + ); + let middle_inter = hand_transform_array[HandJoint::MIDDLE_INTERMEDIATE]; + draw_joint( + &mut gizmos, + middle_inter.translation, + middle_inter.rotation, + 0.006, + Color::YELLOW, + controller_backward, + hand_translation, + ); + let middle_dist = hand_transform_array[HandJoint::MIDDLE_DISTAL]; + draw_joint( + &mut gizmos, + middle_dist.translation, + middle_dist.rotation, + 0.004, + Color::YELLOW, + controller_backward, + hand_translation, + ); + let middle_tip = hand_transform_array[HandJoint::MIDDLE_TIP]; + draw_joint( + &mut gizmos, + middle_tip.translation, + middle_tip.rotation, + 0.002, + Color::YELLOW, + controller_backward, + hand_translation, + ); - let thumb_prox = hand_pose[HandJoint::THUMB_PROXIMAL]; - draw_joint(&mut gizmos, thumb_prox.position.to_vec3(), thumb_prox.orientation.to_quat(), 0.008, Color::RED, controller_backward, hand_translation); - let thumb_dist = hand_pose[HandJoint::THUMB_DISTAL]; - draw_joint(&mut gizmos, thumb_dist.position.to_vec3(), thumb_dist.orientation.to_quat(), 0.006, Color::RED, controller_backward, hand_translation); - let thumb_tip = hand_pose[HandJoint::THUMB_TIP]; - draw_joint(&mut gizmos, thumb_tip.position.to_vec3(), thumb_tip.orientation.to_quat(), 0.004, Color::RED, controller_backward, hand_translation); + let ring_meta = hand_transform_array[HandJoint::RING_METACARPAL]; + draw_joint( + &mut gizmos, + ring_meta.translation, + ring_meta.rotation, + 0.01, + Color::GREEN, + controller_backward, + hand_translation, + ); + let ring_prox = hand_transform_array[HandJoint::RING_PROXIMAL]; + draw_joint( + &mut gizmos, + ring_prox.translation, + ring_prox.rotation, + 0.008, + Color::GREEN, + controller_backward, + hand_translation, + ); + let ring_inter = hand_transform_array[HandJoint::RING_INTERMEDIATE]; + draw_joint( + &mut gizmos, + ring_inter.translation, + ring_inter.rotation, + 0.006, + Color::GREEN, + controller_backward, + hand_translation, + ); + let ring_dist = hand_transform_array[HandJoint::RING_DISTAL]; + draw_joint( + &mut gizmos, + ring_dist.translation, + ring_dist.rotation, + 0.004, + Color::GREEN, + controller_backward, + hand_translation, + ); + let ring_tip = hand_transform_array[HandJoint::RING_TIP]; + draw_joint( + &mut gizmos, + ring_tip.translation, + ring_tip.rotation, + 0.002, + Color::GREEN, + controller_backward, + hand_translation, + ); - let index_meta = hand_pose[HandJoint::INDEX_METACARPAL]; - draw_joint(&mut gizmos, index_meta.position.to_vec3(), index_meta.orientation.to_quat(), 0.01, Color::ORANGE, controller_backward, hand_translation); - let index_prox = hand_pose[HandJoint::INDEX_PROXIMAL]; - draw_joint(&mut gizmos, index_prox.position.to_vec3(), index_prox.orientation.to_quat(), 0.008, Color::ORANGE, controller_backward, hand_translation); - let index_inter = hand_pose[HandJoint::INDEX_INTERMEDIATE]; - draw_joint(&mut gizmos, index_inter.position.to_vec3(), index_inter.orientation.to_quat(), 0.006, Color::ORANGE, controller_backward, hand_translation); - let index_dist = hand_pose[HandJoint::INDEX_DISTAL]; - draw_joint(&mut gizmos, index_dist.position.to_vec3(), index_dist.orientation.to_quat(), 0.004, Color::ORANGE, controller_backward, hand_translation); - let index_tip = hand_pose[HandJoint::INDEX_TIP]; - draw_joint(&mut gizmos, index_tip.position.to_vec3(), index_tip.orientation.to_quat(), 0.002, Color::ORANGE, controller_backward, hand_translation); - - let middle_meta = hand_pose[HandJoint::MIDDLE_METACARPAL]; - draw_joint(&mut gizmos, middle_meta.position.to_vec3(), middle_meta.orientation.to_quat(), 0.01, Color::YELLOW, controller_backward, hand_translation); - let middle_prox = hand_pose[HandJoint::MIDDLE_PROXIMAL]; - draw_joint(&mut gizmos, middle_prox.position.to_vec3(), middle_prox.orientation.to_quat(), 0.008, Color::YELLOW, controller_backward, hand_translation); - let middle_inter = hand_pose[HandJoint::MIDDLE_INTERMEDIATE]; - draw_joint(&mut gizmos, middle_inter.position.to_vec3(), middle_inter.orientation.to_quat(), 0.006, Color::YELLOW, controller_backward, hand_translation); - let middle_dist = hand_pose[HandJoint::MIDDLE_DISTAL]; - draw_joint(&mut gizmos, middle_dist.position.to_vec3(), middle_dist.orientation.to_quat(), 0.004, Color::YELLOW, controller_backward, hand_translation); - let middle_tip = hand_pose[HandJoint::MIDDLE_TIP]; - draw_joint(&mut gizmos, middle_tip.position.to_vec3(), middle_tip.orientation.to_quat(), 0.002, Color::YELLOW, controller_backward, hand_translation); - - let ring_meta = hand_pose[HandJoint::RING_METACARPAL]; - draw_joint(&mut gizmos, ring_meta.position.to_vec3(), ring_meta.orientation.to_quat(), 0.01, Color::GREEN, controller_backward, hand_translation); - let ring_prox = hand_pose[HandJoint::RING_PROXIMAL]; - draw_joint(&mut gizmos, ring_prox.position.to_vec3(), ring_prox.orientation.to_quat(), 0.008, Color::GREEN, controller_backward, hand_translation); - let ring_inter = hand_pose[HandJoint::RING_INTERMEDIATE]; - draw_joint(&mut gizmos, ring_inter.position.to_vec3(), ring_inter.orientation.to_quat(), 0.006, Color::GREEN, controller_backward, hand_translation); - let ring_dist = hand_pose[HandJoint::RING_DISTAL]; - draw_joint(&mut gizmos, ring_dist.position.to_vec3(), ring_dist.orientation.to_quat(), 0.004, Color::GREEN, controller_backward, hand_translation); - let ring_tip = hand_pose[HandJoint::RING_TIP]; - draw_joint(&mut gizmos, ring_tip.position.to_vec3(), ring_tip.orientation.to_quat(), 0.002, Color::GREEN, controller_backward, hand_translation); - - let little_meta = hand_pose[HandJoint::LITTLE_METACARPAL]; - draw_joint(&mut gizmos, little_meta.position.to_vec3(), little_meta.orientation.to_quat(), 0.01, Color::BLUE, controller_backward, hand_translation); - let little_prox = hand_pose[HandJoint::LITTLE_PROXIMAL]; - draw_joint(&mut gizmos, little_prox.position.to_vec3(), little_prox.orientation.to_quat(), 0.008, Color::BLUE, controller_backward, hand_translation); - let little_inter = hand_pose[HandJoint::LITTLE_INTERMEDIATE]; - draw_joint(&mut gizmos, little_inter.position.to_vec3(), little_inter.orientation.to_quat(), 0.006, Color::BLUE, controller_backward, hand_translation); - let little_dist = hand_pose[HandJoint::LITTLE_DISTAL]; - draw_joint(&mut gizmos, little_dist.position.to_vec3(), little_dist.orientation.to_quat(), 0.004, Color::BLUE, controller_backward, hand_translation); - let little_tip = hand_pose[HandJoint::LITTLE_TIP]; - draw_joint(&mut gizmos, little_tip.position.to_vec3(), little_tip.orientation.to_quat(), 0.002, Color::BLUE, controller_backward, hand_translation); + let little_meta = hand_transform_array[HandJoint::LITTLE_METACARPAL]; + draw_joint( + &mut gizmos, + little_meta.translation, + little_meta.rotation, + 0.01, + Color::BLUE, + controller_backward, + hand_translation, + ); + let little_prox = hand_transform_array[HandJoint::LITTLE_PROXIMAL]; + draw_joint( + &mut gizmos, + little_prox.translation, + little_prox.rotation, + 0.008, + Color::BLUE, + controller_backward, + hand_translation, + ); + let little_inter = hand_transform_array[HandJoint::LITTLE_INTERMEDIATE]; + draw_joint( + &mut gizmos, + little_inter.translation, + little_inter.rotation, + 0.006, + Color::BLUE, + controller_backward, + hand_translation, + ); + let little_dist = hand_transform_array[HandJoint::LITTLE_DISTAL]; + draw_joint( + &mut gizmos, + little_dist.translation, + little_dist.rotation, + 0.004, + Color::BLUE, + controller_backward, + hand_translation, + ); + let little_tip = hand_transform_array[HandJoint::LITTLE_TIP]; + draw_joint( + &mut gizmos, + little_tip.translation, + little_tip.rotation, + 0.002, + Color::BLUE, + controller_backward, + hand_translation, + ); } fn flip_hand_pose(hand_pose: [Posef; 26], hand: Hand) -> [Posef; 26] { @@ -234,52 +676,164 @@ fn flip_hand_pose(hand_pose: [Posef; 26], hand: Hand) -> [Posef; 26] { for pose in new_pose.iter_mut() { pose.position.x = -pose.position.x; } - }, + } Hand::Right => (), } return new_pose; } -fn draw_joint(gizmos: &mut Gizmos, joint_pos: Vec3, joint_rot: Quat, radius: f32, color: Color, controller_backwards: Quat, offset: Vec3) { - gizmos.sphere(controller_backwards.mul_vec3(joint_pos) + offset, joint_rot, radius, color); +fn draw_joint( + gizmos: &mut Gizmos, + joint_pos: Vec3, + joint_rot: Quat, + radius: f32, + color: Color, + controller_backwards: Quat, + offset: Vec3, +) { + gizmos.sphere( + controller_backwards.mul_vec3(joint_pos) + offset, + joint_rot, + radius, + color, + ); } fn log_hand(hand_pose: [Posef; 26]) { - let palm_vec = hand_pose[HandJoint::PALM].position.to_vec3(); - info!("palm: {}", hand_pose[HandJoint::PALM].position.to_vec3() - palm_vec); - info!("wrist: {}", hand_pose[HandJoint::WRIST].position.to_vec3() - palm_vec); + 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!("tm: {}", hand_pose[HandJoint::THUMB_METACARPAL].position.to_vec3() - palm_vec); - info!("tp: {}", hand_pose[HandJoint::THUMB_PROXIMAL].position.to_vec3() - palm_vec); - info!("td: {}", hand_pose[HandJoint::THUMB_DISTAL].position.to_vec3() - palm_vec); - info!("tt: {}", hand_pose[HandJoint::THUMB_TIP].position.to_vec3() - palm_vec); - - info!("im: {}", hand_pose[HandJoint::INDEX_METACARPAL].position.to_vec3() - palm_vec); - info!("ip: {}", hand_pose[HandJoint::INDEX_PROXIMAL].position.to_vec3() - palm_vec); - info!("ii: {}", hand_pose[HandJoint::INDEX_INTERMEDIATE].position.to_vec3() - palm_vec); - info!("id: {}", hand_pose[HandJoint::INDEX_DISTAL].position.to_vec3() - palm_vec); - info!("it: {}", hand_pose[HandJoint::INDEX_TIP].position.to_vec3() - palm_vec); + 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!("mm: {}", hand_pose[HandJoint::MIDDLE_METACARPAL].position.to_vec3() - palm_vec); - info!("mp: {}", hand_pose[HandJoint::MIDDLE_PROXIMAL].position.to_vec3() - palm_vec); - info!("mi: {}", hand_pose[HandJoint::MIDDLE_INTERMEDIATE].position.to_vec3() - palm_vec); - info!("md: {}", hand_pose[HandJoint::MIDDLE_DISTAL].position.to_vec3() - palm_vec); - info!("mt: {}", hand_pose[HandJoint::MIDDLE_TIP].position.to_vec3() - palm_vec); + 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!("rm: {}", hand_pose[HandJoint::RING_METACARPAL].position.to_vec3() - palm_vec); - info!("rp: {}", hand_pose[HandJoint::RING_PROXIMAL].position.to_vec3() - palm_vec); - info!("ri: {}", hand_pose[HandJoint::RING_INTERMEDIATE].position.to_vec3() - palm_vec); - info!("rd: {}", hand_pose[HandJoint::RING_DISTAL].position.to_vec3() - palm_vec); - info!("rt: {}", hand_pose[HandJoint::RING_TIP].position.to_vec3() - palm_vec); + 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!("lm: {}", hand_pose[HandJoint::LITTLE_METACARPAL].position.to_vec3() - palm_vec); - info!("lp: {}", hand_pose[HandJoint::LITTLE_PROXIMAL].position.to_vec3() - palm_vec); - info!("li: {}", hand_pose[HandJoint::LITTLE_INTERMEDIATE].position.to_vec3() - palm_vec); - info!("ld: {}", hand_pose[HandJoint::LITTLE_DISTAL].position.to_vec3() - palm_vec); - info!("lt: {}", hand_pose[HandJoint::LITTLE_TIP].position.to_vec3() - palm_vec); + 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() + ); } - fn spawn_controllers_example(mut commands: Commands) { //left hand commands.spawn(( From dd4551c8441b7094114170811c36cbf799db0a3a Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Sat, 7 Oct 2023 18:38:25 -0400 Subject: [PATCH 59/66] hand state resource is used to drive skeleton --- examples/xr.rs | 328 +++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 293 insertions(+), 35 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index f011a96..0e275d9 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -10,6 +10,7 @@ use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin}; use bevy::prelude::*; use bevy::transform::components::Transform; <<<<<<< HEAD +<<<<<<< HEAD use bevy_openxr::xr_input::{QuatConv, Vec3Conv}; use bevy_openxr::xr_input::hand::{OpenXrHandInput, HandInputDebugRenderer}; @@ -17,6 +18,14 @@ use bevy_openxr::xr_input::hand::{OpenXrHandInput, HandInputDebugRenderer}; use bevy_openxr::xr_input::{Vec3Conv, QuatConv, Hand}; use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; >>>>>>> 68cdf19 (both hands work) +======= +use bevy::{gizmos, prelude::*}; +use bevy_openxr::input::XrInput; +use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; +use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; +use bevy_openxr::xr_input::hand_poses::*; +use bevy_openxr::xr_input::oculus_touch::OculusController; +>>>>>>> 319a2dc (hand state resource is used to drive skeleton) use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig}; use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, @@ -40,11 +49,17 @@ fn main() { ======= .add_systems(Startup, spawn_controllers_example) .add_systems(Update, draw_skeleton_hands) +<<<<<<< HEAD >>>>>>> 68cdf19 (both hands work) .insert_resource(PrototypeLocomotionConfig::default()) .add_systems(Startup, spawn_controllers_example) .add_plugins(OpenXrHandInput) .add_plugins(HandInputDebugRenderer) +======= + .add_systems(PreUpdate, update_hand_states) + .insert_resource(PrototypeLocomotionConfig::default()) + .insert_resource(HandStatesResource::default()) +>>>>>>> 319a2dc (hand state resource is used to drive skeleton) .run(); } @@ -98,25 +113,144 @@ fn setup( },)); } +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"), + } +} + fn draw_skeleton_hands( mut commands: Commands, mut gizmos: Gizmos, right_controller_query: Query<(&GlobalTransform, With)>, left_controller_query: Query<(&GlobalTransform, With)>, + hand_states_option: Option>, ) { - let left_hand_transform = left_controller_query - .get_single() - .unwrap() - .0 - .compute_transform(); - draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left); - let right_hand_transform = right_controller_query - .get_single() - .unwrap() - .0 - .compute_transform(); - // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); - draw_hand_bones(&mut gizmos, right_hand_transform, Hand::Right); + match hand_states_option { + Some(hands) => { + let left_hand_transform = left_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left, hands.left); + let right_hand_transform = right_controller_query + .get_single() + .unwrap() + .0 + .compute_transform(); + // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); + draw_hand_bones(&mut gizmos, right_hand_transform, Hand::Right, hands.right); + } + None => info!("hand states resource not initialized yet"), + } } fn pose_array_to_transform_array(hand_pose: [Posef; 26]) -> [Transform; 26] { @@ -130,8 +264,108 @@ fn pose_array_to_transform_array(hand_pose: [Posef; 26]) -> [Transform; 26] { } return result_array; } +#[derive(Clone, Copy)] +pub enum ButtonState { + OFF, + TOUCHED, + PRESSED, +} -fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: Hand) { +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 { + grip: f32, + trigger_state: TriggerState, + a_button: ButtonState, + b_button: ButtonState, + thumbstick: ThumbstickState, +} + +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(), + } + } +} + +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; + } +} + +fn draw_hand_bones( + mut gizmos: &mut Gizmos, + controller_transform: Transform, + hand: Hand, + hand_state: HandState, +) { let left_hand_rot = Quat::from_rotation_y(180.0 * PI / 180.0); let hand_translation: Vec3 = match hand { Hand::Left => controller_transform.translation, @@ -147,26 +381,30 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han Hand::Left => -1.0, Hand::Right => 1.0, }; + //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 paml 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); //draw debug rays - gizmos.ray( - hand_translation, - palm_quat.mul_vec3(Vec3::Z * 0.2), - Color::BLUE, - ); - gizmos.ray( - hand_translation, - palm_quat.mul_vec3(Vec3::Y * 0.2), - Color::GREEN, - ); - gizmos.ray( - hand_translation, - palm_quat.mul_vec3(Vec3::X * 0.2), - Color::RED, - ); + // gizmos.ray( + // hand_translation, + // palm_quat.mul_vec3(Vec3::Z * 0.2), + // Color::BLUE, + // ); + // gizmos.ray( + // hand_translation, + // palm_quat.mul_vec3(Vec3::Y * 0.2), + // Color::GREEN, + // ); + // gizmos.ray( + // hand_translation, + // palm_quat.mul_vec3(Vec3::X * 0.2), + // Color::RED, + // ); //get simulated bones let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); //draw controller-palm bone(should be zero length) @@ -193,11 +431,13 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han let mut prior_vector: Option = None; let color = Color::RED; let splay = Quat::from_rotation_y(splay_direction * 30.0 * PI / 180.0); - let splay_quat = palm_quat.mul_quat(splay); + 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 tp_lrot = Quat::from_rotation_y(splay_direction * 5.0 * PI / 180.0); + 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(); @@ -221,6 +461,7 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han } } + //index //better finger drawing? let thumb_joints = [ HandJoint::INDEX_METACARPAL, @@ -238,7 +479,8 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han for bone in thumb_joints.iter() { match prior_start { Some(start) => { - let tp_lrot = Quat::from_rotation_x(-5.0 * PI / 180.0); + 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(); @@ -279,7 +521,8 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han for bone in thumb_joints.iter() { match prior_start { Some(start) => { - let tp_lrot = Quat::from_rotation_x(-5.0 * PI / 180.0); + 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(); @@ -319,7 +562,8 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han for bone in thumb_joints.iter() { match prior_start { Some(start) => { - let tp_lrot = Quat::from_rotation_x(-5.0 * PI / 180.0); + 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(); @@ -360,7 +604,8 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han for bone in thumb_joints.iter() { match prior_start { Some(start) => { - let tp_lrot = Quat::from_rotation_x(-5.0 * PI / 180.0); + 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(); @@ -385,6 +630,19 @@ fn draw_hand_bones(mut gizmos: &mut Gizmos, controller_transform: Transform, han } } +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 draw_hand(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: Hand) { //draw debug for controller grip center to match palm to let hand_translation = controller_transform.translation; From 7ad02e269f25109e70660e32dbe66fe9f26131e0 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 00:11:23 -0400 Subject: [PATCH 60/66] spawn bones and update --- examples/xr.rs | 556 ++++++++++++++++++++----------------------------- 1 file changed, 229 insertions(+), 327 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 0e275d9..7cd7009 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -57,6 +57,8 @@ fn main() { .add_plugins(HandInputDebugRenderer) ======= .add_systems(PreUpdate, update_hand_states) + .add_systems(PostUpdate, draw_hand_entities) + .add_systems(Startup, spawn_hand_entities) .insert_resource(PrototypeLocomotionConfig::default()) .insert_resource(HandStatesResource::default()) >>>>>>> 319a2dc (hand state resource is used to drive skeleton) @@ -113,6 +115,49 @@ fn setup( },)); } +pub fn spawn_hand_entities(mut commands: Commands) { + let hands = [Hand::Left, Hand::Right]; + let bones = [ + HandBone::PALM, + HandBone::WRIST, + HandBone::THUMB_METACARPAL, + HandBone::THUMB_PROXIMAL, + HandBone::THUMB_DISTAL, + HandBone::THUMB_TIP, + HandBone::INDEX_METACARPAL, + HandBone::INDEX_PROXIMAL, + HandBone::INDEX_INTERMEDIATE, + HandBone::INDEX_DISTAL, + HandBone::INDEX_TIP, + HandBone::MIDDLE_METACARPAL, + HandBone::MIDDLE_PROXIMAL, + HandBone::MIDDLE_INTERMEDIATE, + HandBone::MIDDLE_DISTAL, + HandBone::MIDDLE_TIP, + HandBone::RING_METACARPAL, + HandBone::RING_PROXIMAL, + HandBone::RING_INTERMEDIATE, + HandBone::RING_DISTAL, + HandBone::RING_TIP, + HandBone::LITTLE_METACARPAL, + HandBone::LITTLE_PROXIMAL, + HandBone::LITTLE_INTERMEDIATE, + HandBone::LITTLE_DISTAL, + HandBone::LITTLE_TIP, + ]; + + for hand in hands.iter() { + for bone in bones.iter() { + commands.spawn(( + SpatialBundle::default(), + bone.clone(), + OpenXRTracker, + hand.clone(), + )); + } + } +} + pub fn update_hand_states( oculus_controller: Res, hand_states_option: Option>, @@ -227,11 +272,11 @@ pub fn update_hand_states( } fn draw_skeleton_hands( - mut commands: Commands, mut gizmos: Gizmos, 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)>, ) { match hand_states_option { Some(hands) => { @@ -240,30 +285,61 @@ fn draw_skeleton_hands( .unwrap() .0 .compute_transform(); - draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left, hands.left); + draw_hand_bones( + &mut gizmos, + left_hand_transform, + Hand::Left, + hands.left, + &mut hand_bone_query, + ); let right_hand_transform = right_controller_query .get_single() .unwrap() .0 .compute_transform(); // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); - draw_hand_bones(&mut gizmos, right_hand_transform, Hand::Right, hands.right); + draw_hand_bones( + &mut gizmos, + right_hand_transform, + Hand::Right, + hands.right, + &mut hand_bone_query, + ); } None => info!("hand states resource not initialized yet"), } } -fn pose_array_to_transform_array(hand_pose: [Posef; 26]) -> [Transform; 26] { - let mut result_array: [Transform; 26] = [Transform::default(); 26]; - for (place, data) in result_array.iter_mut().zip(hand_pose.iter()) { - *place = Transform { - translation: data.position.to_vec3(), - rotation: data.orientation.to_quat(), - scale: Vec3::splat(1.0), - } - } - return result_array; +#[derive(Component, Debug, Clone, Copy)] +pub enum HandBone { + PALM, + WRIST, + THUMB_METACARPAL, + THUMB_PROXIMAL, + THUMB_DISTAL, + THUMB_TIP, + INDEX_METACARPAL, + INDEX_PROXIMAL, + INDEX_INTERMEDIATE, + INDEX_DISTAL, + INDEX_TIP, + MIDDLE_METACARPAL, + MIDDLE_PROXIMAL, + MIDDLE_INTERMEDIATE, + MIDDLE_DISTAL, + MIDDLE_TIP, + RING_METACARPAL, + RING_PROXIMAL, + RING_INTERMEDIATE, + RING_DISTAL, + RING_TIP, + LITTLE_METACARPAL, + LITTLE_PROXIMAL, + LITTLE_INTERMEDIATE, + LITTLE_DISTAL, + LITTLE_TIP, } + #[derive(Clone, Copy)] pub enum ButtonState { OFF, @@ -360,11 +436,50 @@ impl HandState { } } +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::THUMB_METACARPAL => (0.01, Color::RED), + HandBone::THUMB_PROXIMAL => (0.008, Color::RED), + HandBone::THUMB_DISTAL => (0.006, Color::RED), + HandBone::THUMB_TIP => (0.004, Color::RED), + HandBone::INDEX_METACARPAL => (0.01, Color::ORANGE), + HandBone::INDEX_PROXIMAL => (0.008, Color::ORANGE), + HandBone::INDEX_INTERMEDIATE => (0.006, Color::ORANGE), + HandBone::INDEX_DISTAL => (0.004, Color::ORANGE), + HandBone::INDEX_TIP => (0.002, Color::ORANGE), + HandBone::MIDDLE_METACARPAL => (0.01, Color::YELLOW), + HandBone::MIDDLE_PROXIMAL => (0.008, Color::YELLOW), + HandBone::MIDDLE_INTERMEDIATE => (0.006, Color::YELLOW), + HandBone::MIDDLE_DISTAL => (0.004, Color::YELLOW), + HandBone::MIDDLE_TIP => (0.002, Color::YELLOW), + HandBone::RING_METACARPAL => (0.01, Color::GREEN), + HandBone::RING_PROXIMAL => (0.008, Color::GREEN), + HandBone::RING_INTERMEDIATE => (0.006, Color::GREEN), + HandBone::RING_DISTAL => (0.004, Color::GREEN), + HandBone::RING_TIP => (0.002, Color::GREEN), + HandBone::LITTLE_METACARPAL => (0.01, Color::BLUE), + HandBone::LITTLE_PROXIMAL => (0.008, Color::BLUE), + HandBone::LITTLE_INTERMEDIATE => (0.006, Color::BLUE), + HandBone::LITTLE_DISTAL => (0.004, Color::BLUE), + HandBone::LITTLE_TIP => (0.002, Color::BLUE), + } +} + fn draw_hand_bones( mut gizmos: &mut Gizmos, 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 { @@ -381,6 +496,9 @@ fn draw_hand_bones( 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(); @@ -410,6 +528,10 @@ fn draw_hand_bones( //draw controller-palm bone(should be zero length) let palm = hand_transform_array[HandJoint::PALM]; gizmos.ray(hand_translation, palm.translation, Color::WHITE); + calc_transforms[HandJoint::PALM] = Transform { + translation: hand_translation + palm.translation, + ..default() + }; //draw palm-wrist let wrist = hand_transform_array[HandJoint::WRIST]; gizmos.ray( @@ -417,6 +539,10 @@ fn draw_hand_bones( palm_quat.mul_vec3(wrist.translation), Color::GRAY, ); + calc_transforms[HandJoint::WRIST] = Transform { + translation: hand_translation + palm.translation + palm_quat.mul_vec3(wrist.translation), + ..default() + }; //thumb //better finger drawing? @@ -446,6 +572,11 @@ fn draw_hand_bones( 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]; @@ -457,6 +588,11 @@ fn draw_hand_bones( 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() + }; } } } @@ -489,6 +625,11 @@ fn draw_hand_bones( 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]; @@ -500,6 +641,11 @@ fn draw_hand_bones( 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() + }; } } } @@ -531,6 +677,11 @@ fn draw_hand_bones( 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]; @@ -542,6 +693,11 @@ fn draw_hand_bones( 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() + }; } } } @@ -572,6 +728,11 @@ fn draw_hand_bones( 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]; @@ -583,6 +744,11 @@ fn draw_hand_bones( 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() + }; } } } @@ -614,6 +780,11 @@ fn draw_hand_bones( 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]; @@ -625,9 +796,54 @@ fn draw_hand_bones( 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::THUMB_METACARPAL => HandJoint::THUMB_METACARPAL, + HandBone::THUMB_PROXIMAL => HandJoint::THUMB_PROXIMAL, + HandBone::THUMB_DISTAL => HandJoint::THUMB_DISTAL, + HandBone::THUMB_TIP => HandJoint::THUMB_TIP, + HandBone::INDEX_METACARPAL => HandJoint::INDEX_METACARPAL, + HandBone::INDEX_PROXIMAL => HandJoint::INDEX_PROXIMAL, + HandBone::INDEX_INTERMEDIATE => HandJoint::INDEX_INTERMEDIATE, + HandBone::INDEX_DISTAL => HandJoint::INDEX_DISTAL, + HandBone::INDEX_TIP => HandJoint::INDEX_TIP, + HandBone::MIDDLE_METACARPAL => HandJoint::MIDDLE_METACARPAL, + HandBone::MIDDLE_PROXIMAL => HandJoint::MIDDLE_PROXIMAL, + HandBone::MIDDLE_INTERMEDIATE => HandJoint::MIDDLE_INTERMEDIATE, + HandBone::MIDDLE_DISTAL => HandJoint::MIDDLE_DISTAL, + HandBone::MIDDLE_TIP => HandJoint::MIDDLE_TIP, + HandBone::RING_METACARPAL => HandJoint::RING_METACARPAL, + HandBone::RING_PROXIMAL => HandJoint::RING_PROXIMAL, + HandBone::RING_INTERMEDIATE => HandJoint::RING_INTERMEDIATE, + HandBone::RING_DISTAL => HandJoint::RING_DISTAL, + HandBone::RING_TIP => HandJoint::RING_TIP, + HandBone::LITTLE_METACARPAL => HandJoint::LITTLE_METACARPAL, + HandBone::LITTLE_PROXIMAL => HandJoint::LITTLE_PROXIMAL, + HandBone::LITTLE_INTERMEDIATE => HandJoint::LITTLE_INTERMEDIATE, + HandBone::LITTLE_DISTAL => HandJoint::LITTLE_DISTAL, + HandBone::LITTLE_TIP => HandJoint::LITTLE_TIP, + } } fn get_bone_curl_angle(bone: HandJoint, curl: f32) -> f32 { @@ -643,320 +859,6 @@ fn get_bone_curl_angle(bone: HandJoint, curl: f32) -> f32 { return curl_angle; } -fn draw_hand(mut gizmos: &mut Gizmos, controller_transform: Transform, hand: Hand) { - //draw debug for controller grip center to match palm to - let hand_translation = controller_transform.translation; - let hand_quat = controller_transform.rotation; - //we need to flip this i dont know why - let flip = Quat::from_rotation_x(PI); - let controller_backward = hand_quat.mul_quat(flip); - - let test_hand_pose = get_test_hand_pose_array(); - - let hand_pose = flip_hand_pose(test_hand_pose.clone(), hand); - - // let hand_transform_array: [Transform; 26] = pose_array_to_transform_array(hand_pose); - let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); - - let palm = hand_transform_array[HandJoint::PALM]; - gizmos.sphere( - palm.translation + hand_translation, - palm.rotation.mul_quat(controller_backward), - 0.01, - Color::WHITE, - ); - - let wrist = hand_transform_array[HandJoint::WRIST]; - draw_joint( - &mut gizmos, - wrist.translation, - wrist.rotation, - 0.01, - Color::GRAY, - controller_backward, - hand_translation, - ); - - let thumb_meta = hand_transform_array[HandJoint::THUMB_METACARPAL]; - draw_joint( - &mut gizmos, - thumb_meta.translation, - thumb_meta.rotation, - 0.01, - Color::RED, - controller_backward, - hand_translation, - ); - - let thumb_prox = hand_transform_array[HandJoint::THUMB_PROXIMAL]; - draw_joint( - &mut gizmos, - thumb_prox.translation, - thumb_prox.rotation, - 0.008, - Color::RED, - controller_backward, - hand_translation, - ); - let thumb_dist = hand_transform_array[HandJoint::THUMB_DISTAL]; - draw_joint( - &mut gizmos, - thumb_dist.translation, - thumb_dist.rotation, - 0.006, - Color::RED, - controller_backward, - hand_translation, - ); - let thumb_tip = hand_transform_array[HandJoint::THUMB_TIP]; - draw_joint( - &mut gizmos, - thumb_tip.translation, - thumb_tip.rotation, - 0.004, - Color::RED, - controller_backward, - hand_translation, - ); - - let index_meta = hand_transform_array[HandJoint::INDEX_METACARPAL]; - draw_joint( - &mut gizmos, - index_meta.translation, - index_meta.rotation, - 0.01, - Color::ORANGE, - controller_backward, - hand_translation, - ); - let index_prox = hand_transform_array[HandJoint::INDEX_PROXIMAL]; - draw_joint( - &mut gizmos, - index_prox.translation, - index_prox.rotation, - 0.008, - Color::ORANGE, - controller_backward, - hand_translation, - ); - - let index_inter = hand_transform_array[HandJoint::INDEX_INTERMEDIATE]; - draw_joint( - &mut gizmos, - index_inter.translation, - index_inter.rotation, - 0.006, - Color::ORANGE, - controller_backward, - hand_translation, - ); - - let index_dist = hand_transform_array[HandJoint::INDEX_DISTAL]; - draw_joint( - &mut gizmos, - index_dist.translation, - index_dist.rotation, - 0.004, - Color::ORANGE, - controller_backward, - hand_translation, - ); - - let index_tip = hand_transform_array[HandJoint::INDEX_TIP]; - draw_joint( - &mut gizmos, - index_tip.translation, - index_tip.rotation, - 0.002, - Color::ORANGE, - controller_backward, - hand_translation, - ); - - let middle_meta = hand_transform_array[HandJoint::MIDDLE_METACARPAL]; - draw_joint( - &mut gizmos, - middle_meta.translation, - middle_meta.rotation, - 0.01, - Color::YELLOW, - controller_backward, - hand_translation, - ); - let middle_prox = hand_transform_array[HandJoint::MIDDLE_PROXIMAL]; - draw_joint( - &mut gizmos, - middle_prox.translation, - middle_prox.rotation, - 0.008, - Color::YELLOW, - controller_backward, - hand_translation, - ); - let middle_inter = hand_transform_array[HandJoint::MIDDLE_INTERMEDIATE]; - draw_joint( - &mut gizmos, - middle_inter.translation, - middle_inter.rotation, - 0.006, - Color::YELLOW, - controller_backward, - hand_translation, - ); - let middle_dist = hand_transform_array[HandJoint::MIDDLE_DISTAL]; - draw_joint( - &mut gizmos, - middle_dist.translation, - middle_dist.rotation, - 0.004, - Color::YELLOW, - controller_backward, - hand_translation, - ); - let middle_tip = hand_transform_array[HandJoint::MIDDLE_TIP]; - draw_joint( - &mut gizmos, - middle_tip.translation, - middle_tip.rotation, - 0.002, - Color::YELLOW, - controller_backward, - hand_translation, - ); - - let ring_meta = hand_transform_array[HandJoint::RING_METACARPAL]; - draw_joint( - &mut gizmos, - ring_meta.translation, - ring_meta.rotation, - 0.01, - Color::GREEN, - controller_backward, - hand_translation, - ); - let ring_prox = hand_transform_array[HandJoint::RING_PROXIMAL]; - draw_joint( - &mut gizmos, - ring_prox.translation, - ring_prox.rotation, - 0.008, - Color::GREEN, - controller_backward, - hand_translation, - ); - let ring_inter = hand_transform_array[HandJoint::RING_INTERMEDIATE]; - draw_joint( - &mut gizmos, - ring_inter.translation, - ring_inter.rotation, - 0.006, - Color::GREEN, - controller_backward, - hand_translation, - ); - let ring_dist = hand_transform_array[HandJoint::RING_DISTAL]; - draw_joint( - &mut gizmos, - ring_dist.translation, - ring_dist.rotation, - 0.004, - Color::GREEN, - controller_backward, - hand_translation, - ); - let ring_tip = hand_transform_array[HandJoint::RING_TIP]; - draw_joint( - &mut gizmos, - ring_tip.translation, - ring_tip.rotation, - 0.002, - Color::GREEN, - controller_backward, - hand_translation, - ); - - let little_meta = hand_transform_array[HandJoint::LITTLE_METACARPAL]; - draw_joint( - &mut gizmos, - little_meta.translation, - little_meta.rotation, - 0.01, - Color::BLUE, - controller_backward, - hand_translation, - ); - let little_prox = hand_transform_array[HandJoint::LITTLE_PROXIMAL]; - draw_joint( - &mut gizmos, - little_prox.translation, - little_prox.rotation, - 0.008, - Color::BLUE, - controller_backward, - hand_translation, - ); - let little_inter = hand_transform_array[HandJoint::LITTLE_INTERMEDIATE]; - draw_joint( - &mut gizmos, - little_inter.translation, - little_inter.rotation, - 0.006, - Color::BLUE, - controller_backward, - hand_translation, - ); - let little_dist = hand_transform_array[HandJoint::LITTLE_DISTAL]; - draw_joint( - &mut gizmos, - little_dist.translation, - little_dist.rotation, - 0.004, - Color::BLUE, - controller_backward, - hand_translation, - ); - let little_tip = hand_transform_array[HandJoint::LITTLE_TIP]; - draw_joint( - &mut gizmos, - little_tip.translation, - little_tip.rotation, - 0.002, - Color::BLUE, - controller_backward, - hand_translation, - ); -} - -fn flip_hand_pose(hand_pose: [Posef; 26], hand: Hand) -> [Posef; 26] { - let mut new_pose = hand_pose; - match hand { - Hand::Left => { - for pose in new_pose.iter_mut() { - pose.position.x = -pose.position.x; - } - } - Hand::Right => (), - } - return new_pose; -} - -fn draw_joint( - gizmos: &mut Gizmos, - joint_pos: Vec3, - joint_rot: Quat, - radius: f32, - color: Color, - controller_backwards: Quat, - offset: Vec3, -) { - gizmos.sphere( - controller_backwards.mul_vec3(joint_pos) + offset, - joint_rot, - radius, - color, - ); -} - fn log_hand(hand_pose: [Posef; 26]) { let palm_wrist = hand_pose[HandJoint::WRIST].position.to_vec3() - hand_pose[HandJoint::PALM].position.to_vec3(); From 7826c445e2294bc1e7b3042b386cf83291412f10 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 00:16:14 -0400 Subject: [PATCH 61/66] slight cleaning --- examples/xr.rs | 64 ++++++++------------------------------------------ 1 file changed, 10 insertions(+), 54 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 7cd7009..184104c 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -271,8 +271,7 @@ pub fn update_hand_states( } } -fn draw_skeleton_hands( - mut gizmos: Gizmos, +fn update_emulated_hand_skeletons( right_controller_query: Query<(&GlobalTransform, With)>, left_controller_query: Query<(&GlobalTransform, With)>, hand_states_option: Option>, @@ -285,8 +284,7 @@ fn draw_skeleton_hands( .unwrap() .0 .compute_transform(); - draw_hand_bones( - &mut gizmos, + update_hand_bones_emulated( left_hand_transform, Hand::Left, hands.left, @@ -297,9 +295,7 @@ fn draw_skeleton_hands( .unwrap() .0 .compute_transform(); - // draw_hand(&mut gizmos, right_hand_transform, Hand::Right); - draw_hand_bones( - &mut gizmos, + update_hand_bones_emulated( right_hand_transform, Hand::Right, hands.right, @@ -474,8 +470,7 @@ fn get_bone_gizmo_style(hand_bone: &HandBone) -> (f32, Color) { } } -fn draw_hand_bones( - mut gizmos: &mut Gizmos, +fn update_hand_bones_emulated( controller_transform: Transform, hand: Hand, hand_state: HandState, @@ -503,49 +498,26 @@ fn draw_hand_bones( let grip_curl = hand_state.grip; let index_curl = hand_state.get_index_curl(); let thumb_curl = hand_state.get_thumb_curl(); - //get paml quat + //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); - //draw debug rays - // gizmos.ray( - // hand_translation, - // palm_quat.mul_vec3(Vec3::Z * 0.2), - // Color::BLUE, - // ); - // gizmos.ray( - // hand_translation, - // palm_quat.mul_vec3(Vec3::Y * 0.2), - // Color::GREEN, - // ); - // gizmos.ray( - // hand_translation, - // palm_quat.mul_vec3(Vec3::X * 0.2), - // Color::RED, - // ); //get simulated bones let hand_transform_array: [Transform; 26] = get_simulated_open_hand_transforms(hand); - //draw controller-palm bone(should be zero length) + //palm let palm = hand_transform_array[HandJoint::PALM]; - gizmos.ray(hand_translation, palm.translation, Color::WHITE); calc_transforms[HandJoint::PALM] = Transform { translation: hand_translation + palm.translation, ..default() }; - //draw palm-wrist + //wrist let wrist = hand_transform_array[HandJoint::WRIST]; - gizmos.ray( - hand_translation + palm.translation, - palm_quat.mul_vec3(wrist.translation), - Color::GRAY, - ); calc_transforms[HandJoint::WRIST] = Transform { translation: hand_translation + palm.translation + palm_quat.mul_vec3(wrist.translation), ..default() }; //thumb - //better finger drawing? let thumb_joints = [ HandJoint::THUMB_METACARPAL, HandJoint::THUMB_PROXIMAL, @@ -555,7 +527,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::RED; 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); @@ -568,7 +539,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -584,7 +554,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); @@ -598,7 +567,6 @@ fn draw_hand_bones( } //index - //better finger drawing? let thumb_joints = [ HandJoint::INDEX_METACARPAL, HandJoint::INDEX_PROXIMAL, @@ -609,7 +577,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::ORANGE; 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() { @@ -621,7 +588,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -637,7 +603,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); @@ -650,7 +615,7 @@ fn draw_hand_bones( } } - //better finger drawing? + //middle let thumb_joints = [ HandJoint::MIDDLE_METACARPAL, HandJoint::MIDDLE_PROXIMAL, @@ -661,7 +626,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::YELLOW; 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() { @@ -673,7 +637,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -689,7 +652,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); @@ -701,7 +663,7 @@ fn draw_hand_bones( } } } - //better finger drawing? + //ring let thumb_joints = [ HandJoint::RING_METACARPAL, HandJoint::RING_PROXIMAL, @@ -712,7 +674,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::GREEN; 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() { @@ -724,7 +685,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -740,7 +700,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); @@ -753,7 +712,7 @@ fn draw_hand_bones( } } - //better finger drawing? + //little let thumb_joints = [ HandJoint::LITTLE_METACARPAL, HandJoint::LITTLE_PROXIMAL, @@ -764,7 +723,6 @@ fn draw_hand_bones( let mut prior_start: Option = None; let mut prior_quat: Option = None; let mut prior_vector: Option = None; - let color = Color::BLUE; 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() { @@ -776,7 +734,6 @@ fn draw_hand_bones( let thumb_prox = hand_transform_array[*bone]; let tp_start = start + prior_vector.unwrap(); let tp_vector = tp_quat.mul_vec3(thumb_prox.translation); - gizmos.ray(tp_start, tp_vector, color); prior_start = Some(tp_start); prior_quat = Some(tp_quat); prior_vector = Some(tp_vector); @@ -792,7 +749,6 @@ fn draw_hand_bones( + palm_quat.mul_vec3(palm.translation) + palm_quat.mul_vec3(wrist.translation); let tm_vector = palm_quat.mul_vec3(thumb_meta.translation); - gizmos.ray(tm_start, tm_vector, color); prior_start = Some(tm_start); prior_quat = Some(splay_quat); prior_vector = Some(tm_vector); From 72961efe31291d4a3aab0ca22c0dd4032031b2b2 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 00:17:31 -0400 Subject: [PATCH 62/66] cargo fix --- examples/xr.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 184104c..d87a00e 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -22,7 +22,7 @@ use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; use bevy::{gizmos, prelude::*}; use bevy_openxr::input::XrInput; use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; -use bevy_openxr::xr_input::debug_gizmos::OpenXrDebugRenderer; + use bevy_openxr::xr_input::hand_poses::*; use bevy_openxr::xr_input::oculus_touch::OculusController; >>>>>>> 319a2dc (hand state resource is used to drive skeleton) @@ -30,9 +30,9 @@ use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLoc use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, }; -use bevy_openxr::xr_input::{Hand, QuatConv, Vec3Conv}; +use bevy_openxr::xr_input::{Hand, Vec3Conv}; use bevy_openxr::DefaultXrPlugins; -use openxr::{HandJoint, Posef, Quaternionf, Vector3f}; +use openxr::{HandJoint, Posef}; fn main() { color_eyre::install().unwrap(); @@ -816,7 +816,7 @@ fn get_bone_curl_angle(bone: HandJoint, curl: f32) -> f32 { } fn log_hand(hand_pose: [Posef; 26]) { - let palm_wrist = hand_pose[HandJoint::WRIST].position.to_vec3() + let _palm_wrist = hand_pose[HandJoint::WRIST].position.to_vec3() - hand_pose[HandJoint::PALM].position.to_vec3(); info!( "palm-wrist: {}", From 6208b2ea4c7e21f222752dbc053a057d9e57ab27 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 00:19:43 -0400 Subject: [PATCH 63/66] mass rename --- examples/xr.rs | 208 ++++++++++++++++++++++++------------------------- 1 file changed, 104 insertions(+), 104 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index d87a00e..6c9e346 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -118,32 +118,32 @@ fn setup( pub fn spawn_hand_entities(mut commands: Commands) { let hands = [Hand::Left, Hand::Right]; let bones = [ - HandBone::PALM, - HandBone::WRIST, - HandBone::THUMB_METACARPAL, - HandBone::THUMB_PROXIMAL, - HandBone::THUMB_DISTAL, - HandBone::THUMB_TIP, - HandBone::INDEX_METACARPAL, - HandBone::INDEX_PROXIMAL, - HandBone::INDEX_INTERMEDIATE, - HandBone::INDEX_DISTAL, - HandBone::INDEX_TIP, - HandBone::MIDDLE_METACARPAL, - HandBone::MIDDLE_PROXIMAL, - HandBone::MIDDLE_INTERMEDIATE, - HandBone::MIDDLE_DISTAL, - HandBone::MIDDLE_TIP, - HandBone::RING_METACARPAL, - HandBone::RING_PROXIMAL, - HandBone::RING_INTERMEDIATE, - HandBone::RING_DISTAL, - HandBone::RING_TIP, - HandBone::LITTLE_METACARPAL, - HandBone::LITTLE_PROXIMAL, - HandBone::LITTLE_INTERMEDIATE, - HandBone::LITTLE_DISTAL, - HandBone::LITTLE_TIP, + 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, ]; for hand in hands.iter() { @@ -308,32 +308,32 @@ fn update_emulated_hand_skeletons( #[derive(Component, Debug, Clone, Copy)] pub enum HandBone { - PALM, - WRIST, - THUMB_METACARPAL, - THUMB_PROXIMAL, - THUMB_DISTAL, - THUMB_TIP, - INDEX_METACARPAL, - INDEX_PROXIMAL, - INDEX_INTERMEDIATE, - INDEX_DISTAL, - INDEX_TIP, - MIDDLE_METACARPAL, - MIDDLE_PROXIMAL, - MIDDLE_INTERMEDIATE, - MIDDLE_DISTAL, - MIDDLE_TIP, - RING_METACARPAL, - RING_PROXIMAL, - RING_INTERMEDIATE, - RING_DISTAL, - RING_TIP, - LITTLE_METACARPAL, - LITTLE_PROXIMAL, - LITTLE_INTERMEDIATE, - LITTLE_DISTAL, - LITTLE_TIP, + 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, } #[derive(Clone, Copy)] @@ -441,32 +441,32 @@ fn draw_hand_entities(mut gizmos: Gizmos, query: Query<(&Transform, &HandBone)>) 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::THUMB_METACARPAL => (0.01, Color::RED), - HandBone::THUMB_PROXIMAL => (0.008, Color::RED), - HandBone::THUMB_DISTAL => (0.006, Color::RED), - HandBone::THUMB_TIP => (0.004, Color::RED), - HandBone::INDEX_METACARPAL => (0.01, Color::ORANGE), - HandBone::INDEX_PROXIMAL => (0.008, Color::ORANGE), - HandBone::INDEX_INTERMEDIATE => (0.006, Color::ORANGE), - HandBone::INDEX_DISTAL => (0.004, Color::ORANGE), - HandBone::INDEX_TIP => (0.002, Color::ORANGE), - HandBone::MIDDLE_METACARPAL => (0.01, Color::YELLOW), - HandBone::MIDDLE_PROXIMAL => (0.008, Color::YELLOW), - HandBone::MIDDLE_INTERMEDIATE => (0.006, Color::YELLOW), - HandBone::MIDDLE_DISTAL => (0.004, Color::YELLOW), - HandBone::MIDDLE_TIP => (0.002, Color::YELLOW), - HandBone::RING_METACARPAL => (0.01, Color::GREEN), - HandBone::RING_PROXIMAL => (0.008, Color::GREEN), - HandBone::RING_INTERMEDIATE => (0.006, Color::GREEN), - HandBone::RING_DISTAL => (0.004, Color::GREEN), - HandBone::RING_TIP => (0.002, Color::GREEN), - HandBone::LITTLE_METACARPAL => (0.01, Color::BLUE), - HandBone::LITTLE_PROXIMAL => (0.008, Color::BLUE), - HandBone::LITTLE_INTERMEDIATE => (0.006, Color::BLUE), - HandBone::LITTLE_DISTAL => (0.004, Color::BLUE), - HandBone::LITTLE_TIP => (0.002, Color::BLUE), + 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), } } @@ -773,32 +773,32 @@ fn update_hand_bones_emulated( fn match_index(handbone: &HandBone) -> HandJoint { match handbone { - HandBone::PALM => HandJoint::PALM, - HandBone::WRIST => HandJoint::WRIST, - HandBone::THUMB_METACARPAL => HandJoint::THUMB_METACARPAL, - HandBone::THUMB_PROXIMAL => HandJoint::THUMB_PROXIMAL, - HandBone::THUMB_DISTAL => HandJoint::THUMB_DISTAL, - HandBone::THUMB_TIP => HandJoint::THUMB_TIP, - HandBone::INDEX_METACARPAL => HandJoint::INDEX_METACARPAL, - HandBone::INDEX_PROXIMAL => HandJoint::INDEX_PROXIMAL, - HandBone::INDEX_INTERMEDIATE => HandJoint::INDEX_INTERMEDIATE, - HandBone::INDEX_DISTAL => HandJoint::INDEX_DISTAL, - HandBone::INDEX_TIP => HandJoint::INDEX_TIP, - HandBone::MIDDLE_METACARPAL => HandJoint::MIDDLE_METACARPAL, - HandBone::MIDDLE_PROXIMAL => HandJoint::MIDDLE_PROXIMAL, - HandBone::MIDDLE_INTERMEDIATE => HandJoint::MIDDLE_INTERMEDIATE, - HandBone::MIDDLE_DISTAL => HandJoint::MIDDLE_DISTAL, - HandBone::MIDDLE_TIP => HandJoint::MIDDLE_TIP, - HandBone::RING_METACARPAL => HandJoint::RING_METACARPAL, - HandBone::RING_PROXIMAL => HandJoint::RING_PROXIMAL, - HandBone::RING_INTERMEDIATE => HandJoint::RING_INTERMEDIATE, - HandBone::RING_DISTAL => HandJoint::RING_DISTAL, - HandBone::RING_TIP => HandJoint::RING_TIP, - HandBone::LITTLE_METACARPAL => HandJoint::LITTLE_METACARPAL, - HandBone::LITTLE_PROXIMAL => HandJoint::LITTLE_PROXIMAL, - HandBone::LITTLE_INTERMEDIATE => HandJoint::LITTLE_INTERMEDIATE, - HandBone::LITTLE_DISTAL => HandJoint::LITTLE_DISTAL, - HandBone::LITTLE_TIP => HandJoint::LITTLE_TIP, + 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, } } From 70eaf184f9f1beae85fd673962b3f49f4132fd1a Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Mon, 9 Oct 2023 21:04:20 -0400 Subject: [PATCH 64/66] create resource to help with entity selection --- examples/xr.rs | 77 +++++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 70 insertions(+), 7 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index 6c9e346..bbf34ee 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -23,6 +23,7 @@ use bevy::{gizmos, prelude::*}; use bevy_openxr::input::XrInput; use bevy_openxr::resources::{XrFrameState, XrInstance, XrSession}; +use bevy_openxr::xr_input::hand::HandsResource; use bevy_openxr::xr_input::hand_poses::*; use bevy_openxr::xr_input::oculus_touch::OculusController; >>>>>>> 319a2dc (hand state resource is used to drive skeleton) @@ -145,17 +146,79 @@ pub fn spawn_hand_entities(mut commands: Commands) { HandBone::LittleDistal, HandBone::LittleTip, ]; - + //hand resource + let mut hand_resource = HandsResource { ..default() }; for hand in hands.iter() { for bone in bones.iter() { - commands.spawn(( - SpatialBundle::default(), - bone.clone(), - OpenXRTracker, - hand.clone(), - )); + 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); } pub fn update_hand_states( From d02d94d383e49926d589de18c8b7a3e505d288ac Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 10 Oct 2023 13:05:09 -0400 Subject: [PATCH 65/66] moved everything to the hand file --- examples/xr.rs | 899 ------------------------------------------------- 1 file changed, 899 deletions(-) diff --git a/examples/xr.rs b/examples/xr.rs index bbf34ee..69b8591 100644 --- a/examples/xr.rs +++ b/examples/xr.rs @@ -31,9 +31,7 @@ use bevy_openxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLoc use bevy_openxr::xr_input::trackers::{ OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker, }; -use bevy_openxr::xr_input::{Hand, Vec3Conv}; use bevy_openxr::DefaultXrPlugins; -use openxr::{HandJoint, Posef}; fn main() { color_eyre::install().unwrap(); @@ -116,903 +114,6 @@ fn setup( },)); } -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); -} - -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"), - } -} - -fn update_emulated_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)>, -) { - 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"), - } -} - -#[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, -} - -#[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 { - grip: f32, - trigger_state: TriggerState, - a_button: ButtonState, - b_button: ButtonState, - thumbstick: ThumbstickState, -} - -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(), - } - } -} - -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; - } -} - -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), - } -} - -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() - ); -} - fn spawn_controllers_example(mut commands: Commands) { //left hand commands.spawn(( From 3fae65ec48aa8c3f3f8cb27b7cbc514ac9eaa305 Mon Sep 17 00:00:00 2001 From: Jay Christy Date: Tue, 17 Oct 2023 22:38:10 -0400 Subject: [PATCH 66/66] comment out the spam --- src/xr_input/hand.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/xr_input/hand.rs b/src/xr_input/hand.rs index f8a70ac..81f5936 100644 --- a/src/xr_input/hand.rs +++ b/src/xr_input/hand.rs @@ -1015,7 +1015,7 @@ pub fn update_hand_skeletons( match input_source { Some(res) => match *res { HandInputSource::Emulated => { - info!("hand input source is emulated"); + // info!("hand input source is emulated"); match hand_states_option { Some(hands) => { let left_hand_transform = left_controller_query