Merge pull request #10 from ForTehLose/prototype_locomotion

Prototype locomotion and refactoring openXR tracking

src/xr_input/debug_gizmos.rs

@@ -1,4 +1,7 @@
-use bevy::prelude::{Color, Gizmos, Plugin, Quat, Res, Transform, Update, Vec2, Vec3};
+use bevy::prelude::{
+    info, Color, Gizmos, GlobalTransform, Plugin, Quat, Query, Res, Transform, Update, Vec2, Vec3,
+    With, Without,
+};
 
 use crate::{
     input::XrInput,
@@ -7,9 +10,11 @@ use crate::{
 
 use crate::xr_input::{
     oculus_touch::{OculusController, OculusControllerRef},
-    Hand, QuatConv, Vec3Conv,
+    Hand,
 };
 
+use super::trackers::{OpenXRLeftController, OpenXRRightController, OpenXRTrackingRoot};
+
 /// add debug renderer for controllers
 #[derive(Default)]
 pub struct OpenXrDebugRenderer;
@@ -27,22 +32,60 @@ pub fn draw_gizmos(
     xr_input: Res<XrInput>,
     instance: Res<XrInstance>,
     session: Res<XrSession>,
+    tracking_root_query: Query<(&mut Transform, With<OpenXRTrackingRoot>)>,
+    left_controller_query: Query<(
+        &GlobalTransform,
+        With<OpenXRLeftController>,
+        Without<OpenXRRightController>,
+        Without<OpenXRTrackingRoot>,
+    )>,
+    right_controller_query: Query<(
+        &GlobalTransform,
+        With<OpenXRRightController>,
+        Without<OpenXRLeftController>,
+        Without<OpenXRTrackingRoot>,
+    )>,
 ) {
     //lock frame
     let frame_state = *frame_state.lock().unwrap();
     //get controller
     let controller = oculus_controller.get_ref(&instance, &session, &frame_state, &xr_input);
+    //tracking root?
+    let mut tracking_transform = &Transform::IDENTITY;
+    let root = tracking_root_query.get_single();
+    match root {
+        Ok(position) => {
+            gizmos.circle(
+                position.0.translation
+                    + Vec3 {
+                        x: 0.0,
+                        y: 0.01,
+                        z: 0.0,
+                    },
+                Vec3::Y,
+                0.2,
+                Color::RED,
+            );
+            tracking_transform = position.0;
+        }
+        Err(_) => info!("too many tracking roots"),
+    }
     //draw the hands
-    draw_hand_gizmo(&mut gizmos, &controller, Hand::Right);
-    draw_hand_gizmo(&mut gizmos, &controller, Hand::Left);
+    let left_transform = left_controller_query.get_single().unwrap().0;
+    let right_transform = right_controller_query.get_single().unwrap().0;
+
+    draw_hand_gizmo(&mut gizmos, &controller, Hand::Right, right_transform);
+    draw_hand_gizmo(&mut gizmos, &controller, Hand::Left, left_transform);
 }
 
-fn draw_hand_gizmo(gizmos: &mut Gizmos, controller: &OculusControllerRef<'_>, hand: Hand) {
+fn draw_hand_gizmo(
+    gizmos: &mut Gizmos,
+    controller: &OculusControllerRef<'_>,
+    hand: Hand,
+    hand_transform: &GlobalTransform,
+) {
     match hand {
         Hand::Left => {
-            //get left controller
-            let left_controller = controller.grip_space(Hand::Left);
-
             let left_color = Color::YELLOW_GREEN;
             let off_color = Color::BLUE;
             let touch_color = Color::GREEN;
@@ -50,9 +93,9 @@ fn draw_hand_gizmo(gizmos: &mut Gizmos, controller: &OculusControllerRef<'_>, ha
 
             let grip_quat_offset = Quat::from_rotation_x(-1.4);
             let face_quat_offset = Quat::from_rotation_x(1.05);
-
-            let controller_vec3 = left_controller.0.pose.position.to_vec3();
-            let controller_quat = left_controller.0.pose.orientation.to_quat();
+            let trans = hand_transform.compute_transform();
+            let controller_vec3 = trans.translation;
+            let controller_quat = trans.rotation;
             let face_quat = controller_quat.mul_quat(face_quat_offset);
             let face_quat_normal = face_quat.mul_vec3(Vec3::Z);
 
@@ -149,7 +192,6 @@ fn draw_hand_gizmo(gizmos: &mut Gizmos, controller: &OculusControllerRef<'_>, ha
         }
         Hand::Right => {
             //get right controller
-            let right_controller = controller.grip_space(Hand::Right);
             let right_color = Color::YELLOW_GREEN;
             let off_color = Color::BLUE;
             let touch_color = Color::GREEN;
@@ -158,11 +200,14 @@ fn draw_hand_gizmo(gizmos: &mut Gizmos, controller: &OculusControllerRef<'_>, ha
             let grip_quat_offset = Quat::from_rotation_x(-1.4);
             let face_quat_offset = Quat::from_rotation_x(1.05);
 
-            let controller_vec3 = right_controller.0.pose.position.to_vec3();
-            let controller_quat = right_controller.0.pose.orientation.to_quat();
+            let trans = hand_transform.compute_transform();
+            let controller_vec3 = trans.translation;
+            let controller_quat = trans.rotation;
             let face_quat = controller_quat.mul_quat(face_quat_offset);
             let face_quat_normal = face_quat.mul_vec3(Vec3::Z);
 
+            let squeeze = controller.squeeze(Hand::Right);
+            //info!("{:?}", squeeze);
             //grip
             gizmos.rect(
                 controller_vec3,

src/xr_input/mod.rs

@@ -1,6 +1,8 @@
 pub mod controllers;
 pub mod debug_gizmos;
 pub mod oculus_touch;
+pub mod prototype_locomotion;
+pub mod trackers;
 pub mod xr_camera;
 
 use crate::resources::XrSession;
@@ -10,12 +12,17 @@ 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::IntoSystemConfigs;
-use bevy::prelude::{Commands, Plugin, PreUpdate, Quat, Res, Vec3};
+use bevy::prelude::{BuildChildren, IntoSystemConfigs};
+use bevy::prelude::{Commands, Plugin, PreUpdate, Quat, Res, SpatialBundle, Update, Vec3};
 use bevy::render::camera::CameraProjectionPlugin;
 use bevy::render::view::{update_frusta, VisibilitySystems};
 use bevy::transform::TransformSystem;
 
+use self::trackers::{
+    adopt_open_xr_trackers, update_open_xr_controllers, OpenXRLeftEye, OpenXRRightEye,
+    OpenXRTrackingRoot,
+};
+
 #[derive(Copy, Clone)]
 pub struct OpenXrInput {
     pub controller_type: XrControllerType,
@@ -25,6 +32,7 @@ pub enum Hand {
     Left,
     Right,
 }
+
 impl OpenXrInput {
     pub fn new(controller_type: XrControllerType) -> Self {
         Self { controller_type }
@@ -39,8 +47,12 @@ impl Plugin for OpenXrInput {
                 app.add_systems(Startup, setup_oculus_controller);
             }
         }
+        //adopt any new trackers
+        app.add_systems(PreUpdate, adopt_open_xr_trackers);
         app.add_systems(PreUpdate, action_set_system);
         app.add_systems(PreUpdate, xr_camera_head_sync.after(xr_begin_frame));
+        //update controller trackers
+        app.add_systems(Update, update_open_xr_controllers);
         app.add_systems(
             PostUpdate,
             update_frusta::<XRProjection>
@@ -52,8 +64,18 @@ impl Plugin for OpenXrInput {
 }
 
 fn setup_xr_cameras(mut commands: Commands) {
-    commands.spawn(XrCameraBundle::new(Eye::Right));
-    commands.spawn(XrCameraBundle::new(Eye::Left));
+    //this needs to do the whole xr tracking volume not just cameras
+    //get the root?
+    let tracking_root = commands
+        .spawn((SpatialBundle::default(), OpenXRTrackingRoot))
+        .id();
+    let right = commands
+        .spawn((XrCameraBundle::new(Eye::Right), OpenXRRightEye))
+        .id();
+    let left = commands
+        .spawn((XrCameraBundle::new(Eye::Left), OpenXRLeftEye))
+        .id();
+    commands.entity(tracking_root).push_children(&[right, left]);
 }
 
 fn action_set_system(action_sets: Res<ActionSets>, session: Res<XrSession>) {

src/xr_input/prototype_locomotion.rs

@@ -0,0 +1,188 @@
+use std::f32::consts::PI;
+
+use bevy::{
+    prelude::*,
+    time::{Time, Timer, TimerMode},
+};
+
+use crate::{
+    input::XrInput,
+    resources::{XrFrameState, XrInstance, XrSession, XrViews},
+};
+
+use super::{
+    oculus_touch::OculusController, trackers::OpenXRTrackingRoot, Hand, QuatConv, Vec3Conv,
+};
+
+pub enum LocomotionType {
+    Head,
+    Hand,
+}
+
+pub enum RotationType {
+    Smooth,
+    Snap,
+}
+
+#[derive(Resource)]
+pub struct RotationTimer {
+    pub timer: Timer,
+}
+
+#[derive(Resource)]
+pub struct PrototypeLocomotionConfig {
+    pub locomotion_type: LocomotionType,
+    pub locomotion_speed: f32,
+    pub rotation_type: RotationType,
+    pub snap_angle: f32,
+    pub smooth_rotation_speed: f32,
+    pub rotation_stick_deadzone: f32,
+    pub rotation_timer: RotationTimer,
+}
+
+impl Default for PrototypeLocomotionConfig {
+    fn default() -> Self {
+        Self {
+            locomotion_type: LocomotionType::Hand,
+            locomotion_speed: 1.0,
+            rotation_type: RotationType::Smooth,
+            snap_angle: 45.0 * (PI / 180.0),
+            smooth_rotation_speed: 0.5 * PI,
+            rotation_stick_deadzone: 0.2,
+            rotation_timer: RotationTimer {
+                timer: Timer::from_seconds(1.0, TimerMode::Once),
+            },
+        }
+    }
+}
+
+pub fn proto_locomotion(
+    time: Res<Time>,
+    mut tracking_root_query: Query<(&mut Transform, With<OpenXRTrackingRoot>)>,
+    oculus_controller: Res<OculusController>,
+    frame_state: Res<XrFrameState>,
+    xr_input: Res<XrInput>,
+    instance: Res<XrInstance>,
+    session: Res<XrSession>,
+    views: ResMut<XrViews>,
+    mut gizmos: Gizmos,
+    config_option: Option<ResMut<PrototypeLocomotionConfig>>,
+) {
+    match config_option {
+        Some(_) => (),
+        None => {
+            info!("no locomotion config");
+            return;
+        }
+    }
+    //i hate this but im too tired to think
+    let mut config = config_option.unwrap();
+    //lock frame
+    let frame_state = *frame_state.lock().unwrap();
+    //get controller
+    let controller = oculus_controller.get_ref(&instance, &session, &frame_state, &xr_input);
+    let root = tracking_root_query.get_single_mut();
+    match root {
+        Ok(mut position) => {
+            //get the stick input and do some maths
+            let stick = controller.thumbstick(Hand::Left);
+            let input = Vec3::new(stick.x, 0.0, -stick.y);
+
+            let mut reference_quat = Quat::IDENTITY;
+            match config.locomotion_type {
+                LocomotionType::Head => {
+                    let v = views.lock().unwrap();
+                    let views = v.get(0);
+                    match views {
+                        Some(view) => {
+                            reference_quat = view
+                                .pose
+                                .orientation
+                                .to_quat()
+                                .mul_quat(position.0.rotation);
+                        }
+                        None => return,
+                    }
+                }
+                LocomotionType::Hand => {
+                    let grip = controller.grip_space(Hand::Left);
+                    reference_quat = grip
+                        .0
+                        .pose
+                        .orientation
+                        .to_quat()
+                        .mul_quat(position.0.rotation);
+                }
+            }
+            //TODO: do this correctly as just removing the y from the resultant vec3 isnt correct, but works well enough for now
+            let mut locomotion_vec = reference_quat.mul_vec3(input);
+            locomotion_vec.y = 0.0;
+            position.0.translation +=
+                locomotion_vec * config.locomotion_speed * time.delta_seconds();
+
+            //now time for rotation
+
+            match config.rotation_type {
+                RotationType::Smooth => {
+                    //once again with the math
+                    let control_stick = controller.thumbstick(Hand::Right);
+                    let rot_input = -control_stick.x; //why is this negative i dont know
+                    if rot_input.abs() <= config.rotation_stick_deadzone {
+                        return;
+                    }
+                    let smoth_rot = Quat::from_rotation_y(
+                        rot_input * config.smooth_rotation_speed * time.delta_seconds(),
+                    );
+                    //apply rotation
+                    let v = views.lock().unwrap();
+                    let views = v.get(0);
+                    match views {
+                        Some(view) => {
+                            let mut hmd_translation = view.pose.position.to_vec3();
+                            hmd_translation.y = 0.0;
+                            let local = position.0.translation;
+                            let global = position.0.rotation.mul_vec3(hmd_translation) + local;
+                            gizmos.circle(global, Vec3::Y, 0.1, Color::GREEN);
+                            position.0.rotate_around(global, smoth_rot);
+                        }
+                        None => return,
+                    }
+                }
+                RotationType::Snap => {
+                    //tick the timer
+                    config.rotation_timer.timer.tick(time.delta());
+                    if config.rotation_timer.timer.finished() {
+                        //now we can snap turn?
+                        //once again with the math
+                        let control_stick = controller.thumbstick(Hand::Right);
+                        let rot_input = -control_stick.x;
+                        if rot_input.abs() <= config.rotation_stick_deadzone {
+                            return;
+                        }
+                        let dir: f32 = match rot_input > 0.0 {
+                            true => 1.0,
+                            false => -1.0,
+                        };
+                        let smoth_rot = Quat::from_rotation_y(config.snap_angle * dir);
+                        //apply rotation
+                        let v = views.lock().unwrap();
+                        let views = v.get(0);
+                        match views {
+                            Some(view) => {
+                                let mut hmd_translation = view.pose.position.to_vec3();
+                                hmd_translation.y = 0.0;
+                                let local = position.0.translation;
+                                let global = position.0.rotation.mul_vec3(hmd_translation) + local;
+                                gizmos.circle(global, Vec3::Y, 0.1, Color::GREEN);
+                                position.0.rotate_around(global, smoth_rot);
+                            }
+                            None => return,
+                        }
+                        config.rotation_timer.timer.reset();
+                    }
+                }
+            }
+        }
+        Err(_) => info!("too many tracking roots"),
+    }
+}

src/xr_input/trackers.rs

@@ -0,0 +1,87 @@
+use bevy::prelude::{Added, BuildChildren, Commands, Entity, Query, With, Res, Transform, Without, Component, info};
+
+use crate::{resources::{XrFrameState, XrInstance, XrSession}, input::XrInput};
+
+use super::{oculus_touch::OculusController, Hand, Vec3Conv, QuatConv};
+
+#[derive(Component)]
+pub struct OpenXRTrackingRoot;
+#[derive(Component)]
+pub struct OpenXRTracker;
+#[derive(Component)]
+pub struct OpenXRLeftEye;
+#[derive(Component)]
+pub struct OpenXRRightEye;
+#[derive(Component)]
+pub struct OpenXRHMD;
+#[derive(Component)]
+pub struct OpenXRLeftController;
+#[derive(Component)]
+pub struct OpenXRRightController;
+#[derive(Component)]
+pub struct OpenXRController;
+
+pub fn adopt_open_xr_trackers(
+    query: Query<(Entity), Added<OpenXRTracker>>,
+    mut commands: Commands,
+    tracking_root_query: Query<(Entity, With<OpenXRTrackingRoot>)>,
+) {
+    let root = tracking_root_query.get_single();
+    match root {
+        Ok(thing) => {
+            // info!("root is");
+            for tracker in query.iter() {
+                info!("we got a new tracker");
+                commands.entity(thing.0).add_child(tracker);
+            }
+        }
+        Err(_) => info!("root isnt spawned yet?"),
+    }
+}
+
+pub fn update_open_xr_controllers(
+    oculus_controller: Res<OculusController>,
+    mut left_controller_query: Query<(
+        &mut Transform,
+        With<OpenXRLeftController>,
+        Without<OpenXRRightController>,
+    )>,
+    mut right_controller_query: Query<(
+        &mut Transform,
+        With<OpenXRRightController>,
+        Without<OpenXRLeftController>,
+    )>,
+    frame_state: Res<XrFrameState>,
+    instance: Res<XrInstance>,
+    xr_input: Res<XrInput>,
+    session: Res<XrSession>,
+) {
+    //lock dat frame?
+    let frame_state = *frame_state.lock().unwrap();
+    //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();
+
+    left_controller_query
+        .get_single_mut()
+        .unwrap()
+        .0
+        .translation = left_postion;
+
+    left_controller_query.get_single_mut().unwrap().0.rotation = left.0.pose.orientation.to_quat();
+    //get right controller
+    let right = controller.grip_space(Hand::Right);
+    let right_postion = right.0.pose.position.to_vec3();
+
+    right_controller_query
+        .get_single_mut()
+        .unwrap()
+        .0
+        .translation = right_postion;
+
+    right_controller_query.get_single_mut().unwrap().0.rotation =
+        right.0.pose.orientation.to_quat();
+}
+

src/xr_input/xr_camera.rs

@@ -46,6 +46,7 @@ pub enum XrCameraType {
     Xr(Eye),
     Flatscreen,
 }
+
 #[derive(Copy, Clone, Debug, Eq, PartialEq, Hash, Ord, PartialOrd)]
 pub enum Eye {
     Left = 0,
@@ -222,6 +223,7 @@ pub fn xr_camera_head_sync(
     mut query: Query<(&mut Transform, &XrCameraType, &mut XRProjection)>,
 ) {
     let mut f = || -> Option<()> {
+        //TODO calculate HMD position
         for (mut transform, camera_type, mut xr_projection) in query.iter_mut() {
             let view_idx = match camera_type {
                 XrCameraType::Xr(eye) => *eye as usize,