fix: openxr playspace mover behavior

src/backend/openxr/mod.rs

@@ -18,7 +18,7 @@ use crate::{
         notifications::NotificationManager,
         openxr::{lines::LinePool, overlay::OpenXrOverlayData},
         overlay::OverlayData,
-        task::TaskType,
+        task::{SystemTask, TaskType},
     },
     graphics::WlxGraphics,
     overlays::{
@@ -45,6 +45,7 @@ struct XrState {
     session: xr::Session<xr::Vulkan>,
     predicted_display_time: xr::Time,
     stage: Arc<xr::Space>,
+    stage_offset: Affine3A,
 }
 
 pub fn openxr_run(running: Arc<AtomicBool>) -> Result<(), BackendError> {
@@ -73,7 +74,7 @@ pub fn openxr_run(running: Arc<AtomicBool>) -> Result<(), BackendError> {
     notifications.run_udp();
 
     let mut delete_queue = vec![];
-    let mut space_mover = playspace::PlayspaceMover::try_new()
+    let mut playspace = playspace::PlayspaceMover::try_new()
         .map_err(|e| log::warn!("Failed to initialize Monado playspace mover: {}", e))
         .ok();
 
@@ -110,6 +111,7 @@ pub fn openxr_run(running: Arc<AtomicBool>) -> Result<(), BackendError> {
         session,
         predicted_display_time: xr::Time::from_nanos(0),
         stage: Arc::new(stage),
+        stage_offset: Affine3A::IDENTITY,
     };
 
     let pointer_lines = [
@@ -206,7 +208,7 @@ pub fn openxr_run(running: Arc<AtomicBool>) -> Result<(), BackendError> {
         }
 
         watch_fade(&mut app_state, overlays.mut_by_id(watch_id).unwrap()); // want panic
-        if let Some(ref mut space_mover) = space_mover {
+        if let Some(ref mut space_mover) = playspace {
             space_mover.update(&mut overlays, &app_state);
         }
 
@@ -370,9 +372,19 @@ pub fn openxr_run(running: Arc<AtomicBool>) -> Result<(), BackendError> {
                         }
                     }
                 }
-                TaskType::System(_task) => {
+                TaskType::System(task) => match task {
-                    // Not implemented
+                    SystemTask::FixFloor => {
-                }
+                        if let Some(ref mut playspace) = playspace {
+                            playspace.fix_floor(&app_state.input_state);
+                        }
+                    }
+                    SystemTask::ResetPlayspace => {
+                        if let Some(ref mut playspace) = playspace {
+                            playspace.reset_offset();
+                        }
+                    }
+                    _ => {}
+                },
             }
         }
 

src/backend/openxr/playspace.rs

@@ -1,20 +1,46 @@
 use std::ffi::c_void;
 
-use glam::Vec3A;
+use glam::{Affine3A, Quat, Vec3A};
 use libloading::{Library, Symbol};
 
-use crate::{backend::common::OverlayContainer, state::AppState};
+use crate::{
+    backend::{common::OverlayContainer, input::InputState},
+    state::AppState,
+};
 
 use super::{helpers, overlay::OpenXrOverlayData};
 
+#[repr(C)]
+struct XrtPose {
+    orientation: [f32; 4],
+    position: [f32; 3],
+}
+
+struct MoverData<T> {
+    pose: Affine3A,
+    hand: usize,
+    hand_pose: T,
+}
+
+// Legacy implementation
+type PlaySpaceMove = extern "C" fn(*mut c_void, f32, f32, f32) -> i32;
+
+// New implementation
+type ApplyStageOffset = extern "C" fn(*mut c_void, *const XrtPose) -> i32;
+
+enum ApiImpl {
+    None,
+    PlaySpaceMove(PlaySpaceMove),
+    ApplyStageOffset(ApplyStageOffset),
+}
+
 pub(super) struct PlayspaceMover {
-    drag_hand: Option<usize>,
+    last_transform: Affine3A,
-    offset: Vec3A,
+    drag: Option<MoverData<Vec3A>>,
-    start_position: Vec3A,
 
     libmonado: Library,
     mnd_root: *mut c_void,
-    playspace_move: extern "C" fn(*mut c_void, f32, f32, f32) -> i32,
+    api_impl: ApiImpl,
 }
 
 impl PlayspaceMover {
@@ -24,9 +50,19 @@ impl PlayspaceMover {
 
             let root_create: Symbol<extern "C" fn(*mut *mut c_void) -> i32> =
                 libmonado.get(b"mnd_root_create\0")?;
-            let playspace_move: Symbol<extern "C" fn(*mut c_void, f32, f32, f32) -> i32> =
+
-                libmonado.get(b"mnd_root_playspace_move\0")?;
+            let mut api_impl = ApiImpl::None;
-            let playspace_move_raw = *playspace_move;
+            if let Ok(playspace_move) = libmonado.get(b"mnd_root_playspace_move\0") {
+                log::info!("Monado: using playspace_move");
+                api_impl = ApiImpl::PlaySpaceMove(*playspace_move);
+            } else if let Ok(apply_stage_offset) = libmonado.get(b"mnd_root_apply_stage_offset\0") {
+                log::info!("Monado: using apply_stage_offset");
+                api_impl = ApiImpl::ApplyStageOffset(*apply_stage_offset);
+            }
+
+            if let ApiImpl::None = api_impl {
+                anyhow::bail!("Monado does not support playspace mover.");
+            }
 
             let mut root: *mut c_void = std::ptr::null_mut();
 
@@ -37,56 +73,108 @@ impl PlayspaceMover {
             }
 
             Ok(Self {
-                drag_hand: None,
+                last_transform: Affine3A::IDENTITY,
-                offset: Vec3A::ZERO,
+                drag: None,
-                start_position: Vec3A::ZERO,
 
                 libmonado,
                 mnd_root: root,
-                playspace_move: playspace_move_raw,
+                api_impl,
             })
         }
     }
 
     pub fn update(&mut self, overlays: &mut OverlayContainer<OpenXrOverlayData>, state: &AppState) {
-        if let Some(hand) = self.drag_hand {
+        if let Some(mut data) = self.drag.take() {
-            let pointer = &state.input_state.pointers[hand];
+            let pointer = &state.input_state.pointers[data.hand];
             if !pointer.now.space_drag {
-                self.drag_hand = None;
+                self.last_transform = data.pose;
                 log::info!("End space drag");
                 return;
             }
 
-            let hand_pos = state.input_state.pointers[hand].pose.translation;
+            let new_hand = data
-            let relative_pos = hand_pos - self.start_position;
+                .pose
+                .transform_point3a(state.input_state.pointers[data.hand].pose.translation);
+            let relative_pos = new_hand - data.hand_pose;
+
+            if relative_pos.length_squared() > 1000.0 {
+                log::warn!("Space drag too fast, ignoring");
+                return;
+            }
+
+            let overlay_offset = data.pose.inverse().transform_vector3a(relative_pos) * -1.0;
 
             overlays.iter_mut().for_each(|overlay| {
                 if overlay.state.grabbable {
                     overlay.state.dirty = true;
-                    overlay.state.transform.translation += relative_pos * -1.0;
+                    overlay.state.transform.translation += overlay_offset;
                 }
             });
 
-            self.offset += relative_pos;
+            data.pose.translation += relative_pos;
-            self.apply_offset();
+            data.hand_pose = new_hand;
+
+            self.apply_offset(data.pose);
+            self.drag = Some(data);
         } else {
             for (i, pointer) in state.input_state.pointers.iter().enumerate() {
-                if pointer.now.space_drag && !pointer.before.space_drag {
+                if pointer.now.space_drag {
+                    let hand_pos = self
+                        .last_transform
+                        .transform_point3a(pointer.pose.translation);
+                    self.drag = Some(MoverData {
+                        pose: self.last_transform,
+                        hand: i,
+                        hand_pose: hand_pos,
+                    });
                     log::info!("Start space drag");
-                    self.drag_hand = Some(i);
+                    return;
-                    self.start_position = pointer.pose.translation;
-                    break;
                 }
             }
         }
     }
 
-    pub fn reset(&mut self) {
+    pub fn reset_offset(&mut self) {
-        self.offset = Vec3A::ZERO;
+        if self.drag.is_some() {
-        self.start_position = Vec3A::ZERO;
+            log::info!("Cannot reset offset while dragging.");
+            return;
+        }
+
+        self.last_transform = Affine3A::IDENTITY;
+        self.apply_offset(self.last_transform);
     }
 
-    fn apply_offset(&mut self) {
+    pub fn fix_floor(&mut self, input: &InputState) {
-        (self.playspace_move)(self.mnd_root, self.offset.x, self.offset.y, self.offset.z);
+        if self.drag.is_some() {
+            log::info!("Cannot fix floor while dragging.");
+            return;
+        }
+
+        let y1 = input.pointers[0].pose.translation.y;
+        let y2 = input.pointers[1].pose.translation.y;
+        let delta = y1.min(y2) - 0.03;
+        self.last_transform.translation.y += delta;
+        self.apply_offset(self.last_transform);
+    }
+
+    fn apply_offset(&self, transform: Affine3A) {
+        match self.api_impl {
+            ApiImpl::PlaySpaceMove(playspace_move) => {
+                (playspace_move)(
+                    self.mnd_root,
+                    transform.translation.x,
+                    transform.translation.y,
+                    transform.translation.z,
+                );
+            }
+            ApiImpl::ApplyStageOffset(apply_stage_offset) => {
+                let xrt_pose = XrtPose {
+                    orientation: Quat::from_affine3(&transform).into(),
+                    position: transform.translation.into(),
+                };
+                (apply_stage_offset)(self.mnd_root, &xrt_pose);
+            }
+            ApiImpl::None => {}
+        }
     }
 }