Enable locomotion to handle arbitrary rotations of the view space (#49)

* Enable locomotion to handle arbitrary rotations of the view space

Previously it assumed that "up" was always the Y axis

* Eliminate jitter

---------

Co-authored-by: Oliver Scherer <github@oli-obk.de>

examples/globe.rs

@@ -0,0 +1,188 @@
+use bevy::diagnostic::LogDiagnosticsPlugin;
+use bevy::prelude::*;
+use bevy::transform::components::Transform;
+use bevy_oxr::resources::XrViews;
+use bevy_oxr::xr_input::hands::common::{HandInputDebugRenderer, OpenXrHandInput};
+use bevy_oxr::xr_input::interactions::{
+    InteractionEvent, XRDirectInteractor, XRInteractorState, XRRayInteractor, XRSocketInteractor,
+};
+use bevy_oxr::xr_input::prototype_locomotion::{proto_locomotion, PrototypeLocomotionConfig};
+use bevy_oxr::xr_input::trackers::{
+    AimPose, OpenXRController, OpenXRLeftController, OpenXRRightController, OpenXRTracker,
+    OpenXRTrackingRoot,
+};
+use bevy_oxr::xr_input::Vec3Conv;
+use bevy_oxr::DefaultXrPlugins;
+use wgpu::{Extent3d, TextureDimension, TextureFormat};
+
+fn main() {
+    color_eyre::install().unwrap();
+
+    App::new()
+        .add_plugins(DefaultXrPlugins)
+        .add_plugins(LogDiagnosticsPlugin::default())
+        .add_systems(Startup, setup)
+        .add_systems(Update, (proto_locomotion, pull_to_ground).chain())
+        .insert_resource(PrototypeLocomotionConfig::default())
+        .add_systems(Startup, spawn_controllers_example)
+        .add_plugins(OpenXrHandInput)
+        .add_plugins(HandInputDebugRenderer)
+        .add_event::<InteractionEvent>()
+        .run();
+}
+
+#[derive(Component)]
+struct Globe {
+    radius: f32,
+}
+
+/// Creates a colorful test pattern
+fn uv_debug_texture() -> Image {
+    const TEXTURE_SIZE: usize = 8;
+
+    let mut palette: [u8; 32] = [
+        255, 102, 159, 255, 255, 159, 102, 255, 236, 255, 102, 255, 121, 255, 102, 255, 102, 255,
+        198, 255, 102, 198, 255, 255, 121, 102, 255, 255, 236, 102, 255, 255,
+    ];
+
+    let mut texture_data = [0; TEXTURE_SIZE * TEXTURE_SIZE * 4];
+    for y in 0..TEXTURE_SIZE {
+        let offset = TEXTURE_SIZE * y * 4;
+        texture_data[offset..(offset + TEXTURE_SIZE * 4)].copy_from_slice(&palette);
+        palette.rotate_right(4);
+    }
+
+    Image::new_fill(
+        Extent3d {
+            width: TEXTURE_SIZE as u32,
+            height: TEXTURE_SIZE as u32,
+            depth_or_array_layers: 1,
+        },
+        TextureDimension::D2,
+        &texture_data,
+        TextureFormat::Rgba8UnormSrgb,
+    )
+}
+
+/// set up a simple 3D scene
+fn setup(
+    mut commands: Commands,
+    mut meshes: ResMut<Assets<Mesh>>,
+    mut materials: ResMut<Assets<StandardMaterial>>,
+    mut images: ResMut<Assets<Image>>,
+) {
+    // plane
+    let radius = 5.0;
+    commands.spawn((
+        PbrBundle {
+            mesh: meshes.add(
+                shape::UVSphere {
+                    radius,
+                    sectors: 10,
+                    stacks: 10,
+                }
+                .try_into()
+                .unwrap(),
+            ),
+            material: materials.add(StandardMaterial {
+                base_color_texture: Some(images.add(uv_debug_texture())),
+                ..default()
+            }),
+            transform: Transform::from_xyz(0.0, -radius, 0.0),
+            ..default()
+        },
+        Globe { radius },
+    ));
+    // cube
+    commands.spawn(PbrBundle {
+        mesh: meshes.add(Mesh::from(shape::Cube { size: 0.1 })),
+        material: materials.add(Color::rgb(0.8, 0.7, 0.6).into()),
+        transform: Transform::from_xyz(0.0, 0.5, 0.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 {
+            intensity: 1500.0,
+            shadows_enabled: true,
+            ..default()
+        },
+        transform: Transform::from_xyz(4.0, 8.0, 4.0),
+        ..default()
+    });
+    // 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,
+        ),
+        ..default()
+    },));
+}
+
+fn spawn_controllers_example(mut commands: Commands) {
+    //left hand
+    commands.spawn((
+        OpenXRLeftController,
+        OpenXRController,
+        OpenXRTracker,
+        SpatialBundle::default(),
+        XRRayInteractor,
+        AimPose(Transform::default()),
+        XRInteractorState::default(),
+    ));
+    //right hand
+    commands.spawn((
+        OpenXRRightController,
+        OpenXRController,
+        OpenXRTracker,
+        SpatialBundle::default(),
+        XRDirectInteractor,
+        XRInteractorState::default(),
+    ));
+}
+
+fn pull_to_ground(
+    time: Res<Time>,
+    mut tracking_root_query: Query<&mut Transform, (With<OpenXRTrackingRoot>, Without<Globe>)>,
+    globe: Query<(&Transform, &Globe), Without<OpenXRTrackingRoot>>,
+    views: ResMut<XrViews>,
+) {
+    let mut root = tracking_root_query.single_mut();
+    let (globe_pos, globe) = globe.single();
+
+    // Get player position (position of playground + position within playground)
+    let v = views.lock().unwrap();
+    let Some(view) = v.get(0) else { return };
+    let mut hmd_translation = view.pose.position.to_vec3();
+    hmd_translation.y = 0.0;
+    let local = root.translation;
+    let offset = root.rotation.mul_vec3(hmd_translation);
+    let global = offset + local;
+
+    let adjustment_rate = (time.delta_seconds() * 10.0).min(1.0);
+
+    // Lower player onto sphere
+    let up = (global - globe_pos.translation).normalize();
+    let diff = up * globe.radius + globe_pos.translation - offset - root.translation;
+    root.translation += diff * adjustment_rate;
+
+    // Rotate player to be upright on sphere
+    let angle_diff = Quat::from_rotation_arc(root.up(), up);
+    let point = root.translation + offset;
+    root.rotate_around(point, Quat::IDENTITY.slerp(angle_diff, adjustment_rate));
+}

src/xr_input/prototype_locomotion.rs

@@ -88,37 +88,27 @@ pub fn proto_locomotion(
         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 input = stick.x * position.0.right() + stick.y * position.0.forward();
-
+            let reference_quat;
-            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
+                            reference_quat = view.pose.orientation.to_quat();
-                                .pose
-                                .orientation
-                                .to_quat()
-                                .mul_quat(position.0.rotation);
                         }
                         None => return,
                     }
                 }
                 LocomotionType::Hand => {
                     let grip = controller.grip_space(Hand::Left);
-                    reference_quat = grip
+                    reference_quat = grip.0.pose.orientation.to_quat();
-                        .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 (yaw, _pitch, _roll) = reference_quat.to_euler(EulerRot::YXZ);
-            let mut locomotion_vec = reference_quat.mul_vec3(input);
+            let reference_quat = Quat::from_axis_angle(position.0.up(), yaw);
-            locomotion_vec.y = 0.0;
+            let locomotion_vec = reference_quat.mul_vec3(input);
             position.0.translation +=
                 locomotion_vec * config.locomotion_speed * time.delta_seconds();
 
@@ -132,7 +122,8 @@ pub fn proto_locomotion(
                     if rot_input.abs() <= config.rotation_stick_deadzone {
                         return;
                     }
-                    let smoth_rot = Quat::from_rotation_y(
+                    let smoth_rot = Quat::from_axis_angle(
+                        position.0.up(),
                         rot_input * config.smooth_rotation_speed * time.delta_seconds(),
                     );
                     //apply rotation
@@ -144,7 +135,7 @@ pub fn proto_locomotion(
                             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);
+                            gizmos.circle(global, position.0.up(), 0.1, Color::GREEN);
                             position.0.rotate_around(global, smoth_rot);
                         }
                         None => return,
@@ -165,7 +156,8 @@ pub fn proto_locomotion(
                             true => 1.0,
                             false => -1.0,
                         };
-                        let smoth_rot = Quat::from_rotation_y(config.snap_angle * dir);
+                        let smoth_rot =
+                            Quat::from_axis_angle(position.0.up(), config.snap_angle * dir);
                         //apply rotation
                         let v = views.lock().unwrap();
                         let views = v.get(0);
@@ -175,7 +167,7 @@ pub fn proto_locomotion(
                                 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);
+                                gizmos.circle(global, position.0.up(), 0.1, Color::GREEN);
                                 position.0.rotate_around(global, smoth_rot);
                             }
                             None => return,