hand state resource is used to drive skeleton

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<OculusController>,
+    hand_states_option: Option<ResMut<HandStatesResource>>,
+    frame_state: Res<XrFrameState>,
+    xr_input: Res<XrInput>,
+    instance: Res<XrInstance>,
+    session: Res<XrSession>,
+) {
+    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<OpenXRRightController>)>,
     left_controller_query: Query<(&GlobalTransform, With<OpenXRLeftController>)>,
+    hand_states_option: Option<ResMut<HandStatesResource>>,
 ) {
-    let left_hand_transform = left_controller_query
+    match hand_states_option {
-        .get_single()
+        Some(hands) => {
-        .unwrap()
+            let left_hand_transform = left_controller_query
-        .0
+                .get_single()
-        .compute_transform();
+                .unwrap()
-    draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left);
+                .0
-    let right_hand_transform = right_controller_query
+                .compute_transform();
-        .get_single()
+            draw_hand_bones(&mut gizmos, left_hand_transform, Hand::Left, hands.left);
-        .unwrap()
+            let right_hand_transform = right_controller_query
-        .0
+                .get_single()
-        .compute_transform();
+                .unwrap()
-    // draw_hand(&mut gizmos, right_hand_transform, Hand::Right);
+                .0
-    draw_hand_bones(&mut gizmos, right_hand_transform, Hand::Right);
+                .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(
+    // gizmos.ray(
-        hand_translation,
+    //     hand_translation,
-        palm_quat.mul_vec3(Vec3::Z * 0.2),
+    //     palm_quat.mul_vec3(Vec3::Z * 0.2),
-        Color::BLUE,
+    //     Color::BLUE,
-    );
+    // );
-    gizmos.ray(
+    // gizmos.ray(
-        hand_translation,
+    //     hand_translation,
-        palm_quat.mul_vec3(Vec3::Y * 0.2),
+    //     palm_quat.mul_vec3(Vec3::Y * 0.2),
-        Color::GREEN,
+    //     Color::GREEN,
-    );
+    // );
-    gizmos.ray(
+    // gizmos.ray(
-        hand_translation,
+    //     hand_translation,
-        palm_quat.mul_vec3(Vec3::X * 0.2),
+    //     palm_quat.mul_vec3(Vec3::X * 0.2),
-        Color::RED,
+    //     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<Vec3> = 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;