Merge pull request #32 from Schmarni-Dev/push_main_2

Add Hand tracking

src/graphics/vulkan.rs

@@ -55,6 +55,9 @@ pub fn initialize_xr_graphics(
     {
         enabled_extensions.khr_android_create_instance = true;
     }
+    enabled_extensions.ext_hand_tracking = available_extensions.ext_hand_tracking;
+    // enabled_extensions.ext_hand_joints_motion_range = available_extensions.ext_hand_joints_motion_range;
+
 
     let available_layers = xr_entry.enumerate_layers()?;
     info!("available xr layers: {:#?}", available_layers);

src/lib.rs

@@ -20,6 +20,7 @@ use input::XrInput;
 use openxr as xr;
 use resources::*;
 use xr_input::controllers::XrControllerType;
+use xr_input::handtracking::HandTrackingTracker;
 use xr_input::OpenXrInput;
 
 const VIEW_TYPE: xr::ViewConfigurationType = xr::ViewConfigurationType::PRIMARY_STEREO;
@@ -143,7 +144,8 @@ impl Plugin for OpenXrPlugin {
                 .insert_resource(input.clone())
                 .insert_resource(views.clone())
                 .insert_resource(frame_state.clone())
-                .insert_resource(action_sets.clone());
+                .insert_resource(action_sets.clone())
+                .insert_resource(HandTrackingTracker::new(&session).unwrap());
 
             let (left, right) = swapchain.get_render_views();
             let left = ManualTextureView {
@@ -330,16 +332,15 @@ pub fn end_frame(
     }
     {
         let _span = info_span!("xr_end_frame").entered();
-        let result = swapchain
-            .end(
-                xr_frame_state.lock().unwrap().predicted_display_time,
-                &*views.lock().unwrap(),
-                &input.stage,
-                **resolution,
-                **environment_blend_mode,
-            );
+        let result = swapchain.end(
+            xr_frame_state.lock().unwrap().predicted_display_time,
+            &*views.lock().unwrap(),
+            &input.stage,
+            **resolution,
+            **environment_blend_mode,
+        );
         match result {
-            Ok(_) => {},
+            Ok(_) => {}
             Err(e) => warn!("error: {}", e),
         }
     }

src/xr_input/debug_gizmos.rs

@@ -13,7 +13,11 @@ use crate::xr_input::{
     Hand,
 };
 
-use super::trackers::{OpenXRLeftController, OpenXRRightController, OpenXRTrackingRoot};
+use super::{
+    handtracking::{HandTrackingRef, HandTrackingTracker},
+    trackers::{OpenXRLeftController, OpenXRRightController, OpenXRTrackingRoot},
+    QuatConv,
+};
 
 /// add debug renderer for controllers
 #[derive(Default)]
@@ -32,7 +36,12 @@ pub fn draw_gizmos(
     xr_input: Res<XrInput>,
     instance: Res<XrInstance>,
     session: Res<XrSession>,
-    tracking_root_query: Query<(&mut Transform, With<OpenXRTrackingRoot>)>,
+    tracking_root_query: Query<(
+        &mut Transform,
+        With<OpenXRTrackingRoot>,
+        Without<OpenXRLeftController>,
+        Without<OpenXRRightController>,
+    )>,
     left_controller_query: Query<(
         &GlobalTransform,
         With<OpenXRLeftController>,
@@ -45,7 +54,40 @@ pub fn draw_gizmos(
         Without<OpenXRLeftController>,
         Without<OpenXRTrackingRoot>,
     )>,
+    hand_tracking: Res<HandTrackingTracker>,
 ) {
+    let handtracking_ref = hand_tracking.get_ref(&xr_input, &frame_state);
+    if let Some(joints) = handtracking_ref.get_left_poses() {
+        for joint in joints {
+            let p = joint.pose.position;
+            let r = joint.pose.orientation;
+            let quat = r.to_quat();
+            let trans = Transform::from_rotation(quat);
+            gizmos.circle(
+                (p.x, p.y, p.z).into(),
+                trans.forward(),
+                joint.radius,
+                Color::ORANGE_RED,
+            );
+        }
+    } else {
+        info!("left_hand_poses returned None");
+    }
+    if let Some(joints) = handtracking_ref.get_right_poses() {
+        for joint in joints {
+            let p = joint.pose.position;
+            let r = joint.pose.orientation;
+            let quat = r.to_quat();
+            let trans = Transform::from_rotation(quat);
+            gizmos.circle(
+                (p.x, p.y, p.z).into(),
+                trans.forward(),
+                joint.radius,
+                Color::LIME_GREEN,
+            );
+        }
+        return;
+    }
     //lock frame
     let frame_state = *frame_state.lock().unwrap();
     //get controller

src/xr_input/hand.rs

@@ -1,9 +1,9 @@
 use std::f32::consts::PI;
 
 use bevy::prelude::{
-    default, info, Color, Commands, Component, Entity, Gizmos, GlobalTransform, Plugin, PostUpdate,
-    PreUpdate, Quat, Query, Res, ResMut, Resource, SpatialBundle, Startup, Transform, Update, Vec3,
-    With,
+    default, info, Color, Commands, Component, Deref, DerefMut, Entity, Gizmos, GlobalTransform,
+    Plugin, PostUpdate, PreUpdate, Quat, Query, Res, ResMut, Resource, SpatialBundle, Startup,
+    Transform, Update, Vec3, With, Without,
 };
 use openxr::{HandJoint, Posef};
 
@@ -15,9 +15,10 @@ use crate::{
 
 use super::{
     hand_poses::get_simulated_open_hand_transforms,
+    handtracking::HandTrackingTracker,
     oculus_touch::OculusController,
-    trackers::{OpenXRLeftController, OpenXRRightController, OpenXRTracker},
-    Hand,
+    trackers::{OpenXRLeftController, OpenXRRightController, OpenXRTracker, OpenXRTrackingRoot},
+    Hand, QuatConv,
 };
 
 /// add debug renderer for controllers
@@ -52,7 +53,7 @@ pub enum HandInputSource {
 
 impl Default for HandInputSource {
     fn default() -> Self {
-        HandInputSource::Emulated
+        HandInputSource::OpenXr
     }
 }
 
@@ -183,34 +184,7 @@ impl Default for LittleResource {
 
 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,
-    ];
+    let bones = HandBone::get_all_bones();
     //hand resource
     let mut hand_resource = HandsResource { ..default() };
     for hand in hands.iter() {
@@ -319,6 +293,68 @@ pub enum HandBone {
     LittleDistal,
     LittleTip,
 }
+impl HandBone {
+    pub const fn get_all_bones() -> [HandBone; 26] {
+        [
+            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,
+        ]
+    }
+    pub fn get_index_from_bone(&self) -> usize {
+        match &self {
+            HandBone::Palm => 0,
+            HandBone::Wrist => 1,
+            HandBone::ThumbMetacarpal => 2,
+            HandBone::ThumbProximal => 3,
+            HandBone::ThumbDistal => 4,
+            HandBone::ThumbTip => 5,
+            HandBone::IndexMetacarpal => 6,
+            HandBone::IndexProximal => 7,
+            HandBone::IndexIntermediate => 8,
+            HandBone::IndexDistal => 9,
+            HandBone::IndexTip => 10,
+            HandBone::MiddleMetacarpal => 11,
+            HandBone::MiddleProximal => 12,
+            HandBone::MiddleIntermediate => 13,
+            HandBone::MiddleDistal => 14,
+            HandBone::MiddleTip => 15,
+            HandBone::RingMetacarpal => 16,
+            HandBone::RingProximal => 17,
+            HandBone::RingIntermediate => 18,
+            HandBone::RingDistal => 19,
+            HandBone::RingTip => 20,
+            HandBone::LittleMetacarpal => 21,
+            HandBone::LittleProximal => 22,
+            HandBone::LittleIntermediate => 23,
+            HandBone::LittleDistal => 24,
+            HandBone::LittleTip => 25,
+        }
+    }
+}
 
 pub fn update_hand_states(
     oculus_controller: Res<OculusController>,
@@ -533,7 +569,15 @@ pub fn update_hand_bones_emulated(
     controller_transform: Transform,
     hand: Hand,
     hand_state: HandState,
-    hand_bone_query: &mut Query<(&mut Transform, &HandBone, &Hand)>,
+
+    hand_bone_query: &mut Query<(
+        Entity,
+        &mut Transform,
+        &HandBone,
+        &Hand,
+        Option<&mut HandBoneRadius>,
+        Without<OpenXRTrackingRoot>,
+    )>,
 ) {
     let left_hand_rot = Quat::from_rotation_y(180.0 * PI / 180.0);
     let hand_translation: Vec3 = match hand {
@@ -821,7 +865,7 @@ pub fn update_hand_bones_emulated(
     }
 
     //now that we have all the transforms lets assign them
-    for (mut transform, handbone, bonehand) in hand_bone_query.iter_mut() {
+    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);
@@ -1010,11 +1054,23 @@ fn log_hand(hand_pose: [Posef; 26]) {
 }
 
 pub fn update_hand_skeletons(
+    tracking_root_query: Query<(&Transform, With<OpenXRTrackingRoot>)>,
     right_controller_query: Query<(&GlobalTransform, With<OpenXRRightController>)>,
     left_controller_query: Query<(&GlobalTransform, With<OpenXRLeftController>)>,
     hand_states_option: Option<ResMut<HandStatesResource>>,
-    mut hand_bone_query: Query<(&mut Transform, &HandBone, &Hand)>,
+    mut commands: Commands,
+    mut hand_bone_query: Query<(
+        Entity,
+        &mut Transform,
+        &HandBone,
+        &Hand,
+        Option<&mut HandBoneRadius>,
+        Without<OpenXRTrackingRoot>,
+    )>,
     input_source: Option<Res<HandInputSource>>,
+    hand_tracking: Res<HandTrackingTracker>,
+    xr_input: Res<XrInput>,
+    xr_frame_state: Res<XrFrameState>,
 ) {
     match input_source {
         Some(res) => match *res {
@@ -1049,8 +1105,33 @@ pub fn update_hand_skeletons(
                 }
             }
             HandInputSource::OpenXr => {
-                info!("hand input source is open XR: this is not implemented yet");
-                return;
+                let hand_ref = hand_tracking.get_ref(&xr_input, &xr_frame_state);
+                let (root_transform, _) = tracking_root_query.get_single().unwrap();
+                let left_data = hand_ref.get_left_poses();
+                let right_data = hand_ref.get_right_poses();
+
+                for (entity, mut transform, bone, hand, radius, _) in hand_bone_query.iter_mut() {
+                    let bone_data = match (hand, left_data, right_data) {
+                        (Hand::Left, Some(data), _) => data[bone.get_index_from_bone()],
+                        (Hand::Right, _, Some(data)) => data[bone.get_index_from_bone()],
+                        _ => continue,
+                    };
+                    match radius {
+                        Some(mut r) => r.0 = bone_data.radius,
+                        None => {
+                            commands
+                                .entity(entity)
+                                .insert(HandBoneRadius(bone_data.radius));
+                        }
+                    }
+                    *transform = transform
+                        .with_translation(
+                            root_transform.transform_point(bone_data.pose.position.to_vec3()),
+                        )
+                        .with_rotation(
+                            root_transform.rotation * bone_data.pose.orientation.to_quat(),
+                        )
+                }
             }
         },
         None => {
@@ -1060,10 +1141,21 @@ pub fn update_hand_skeletons(
     }
 }
 
-pub fn draw_hand_entities(mut gizmos: Gizmos, query: Query<(&Transform, &HandBone)>) {
-    for (transform, hand_bone) in query.iter() {
+#[derive(Debug, Component, DerefMut, Deref)]
+pub struct HandBoneRadius(pub f32);
+
+pub fn draw_hand_entities(
+    mut gizmos: Gizmos,
+    query: Query<(&Transform, &HandBone, Option<&HandBoneRadius>)>,
+) {
+    for (transform, hand_bone, hand_bone_radius) in query.iter() {
         let (radius, color) = get_bone_gizmo_style(hand_bone);
-        gizmos.sphere(transform.translation, transform.rotation, radius, color);
+        gizmos.sphere(
+            transform.translation,
+            transform.rotation,
+            hand_bone_radius.map_or(radius, |r| r.0),
+            color,
+        );
     }
 }