From 65d62d79fe5f4b729473210cf85d20b4bebb8ab8 Mon Sep 17 00:00:00 2001 From: galister <22305755+galister@users.noreply.github.com> Date: Tue, 4 Jun 2024 21:23:04 +0900 Subject: [PATCH] fix: openxr playspace mover behavior --- src/backend/openxr/mod.rs | 24 ++++-- src/backend/openxr/playspace.rs | 148 +++++++++++++++++++++++++------- 2 files changed, 136 insertions(+), 36 deletions(-) diff --git a/src/backend/openxr/mod.rs b/src/backend/openxr/mod.rs index 8fdb79f..dcc1dab 100644 --- a/src/backend/openxr/mod.rs +++ b/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, predicted_display_time: xr::Time, stage: Arc, + stage_offset: Affine3A, } pub fn openxr_run(running: Arc) -> Result<(), BackendError> { @@ -73,7 +74,7 @@ pub fn openxr_run(running: Arc) -> 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) -> 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) -> 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) -> Result<(), BackendError> { } } } - TaskType::System(_task) => { - // Not implemented - } + TaskType::System(task) => match task { + 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(); + } + } + _ => {} + }, } } diff --git a/src/backend/openxr/playspace.rs b/src/backend/openxr/playspace.rs index fe1dbd0..f801f55 100644 --- a/src/backend/openxr/playspace.rs +++ b/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 { + 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, - offset: Vec3A, - start_position: Vec3A, + last_transform: Affine3A, + drag: Option>, 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 i32> = libmonado.get(b"mnd_root_create\0")?; - let playspace_move: Symbol i32> = - libmonado.get(b"mnd_root_playspace_move\0")?; - let playspace_move_raw = *playspace_move; + + let mut api_impl = ApiImpl::None; + 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, - offset: Vec3A::ZERO, - start_position: Vec3A::ZERO, + last_transform: Affine3A::IDENTITY, + drag: None, libmonado, mnd_root: root, - playspace_move: playspace_move_raw, + api_impl, }) } } pub fn update(&mut self, overlays: &mut OverlayContainer, state: &AppState) { - if let Some(hand) = self.drag_hand { - let pointer = &state.input_state.pointers[hand]; + if let Some(mut data) = self.drag.take() { + 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 relative_pos = hand_pos - self.start_position; + let new_hand = data + .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; - self.apply_offset(); + data.pose.translation += relative_pos; + 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); - self.start_position = pointer.pose.translation; - break; + return; } } } } - pub fn reset(&mut self) { - self.offset = Vec3A::ZERO; - self.start_position = Vec3A::ZERO; + pub fn reset_offset(&mut self) { + if self.drag.is_some() { + log::info!("Cannot reset offset while dragging."); + return; + } + + self.last_transform = Affine3A::IDENTITY; + self.apply_offset(self.last_transform); } - fn apply_offset(&mut self) { - (self.playspace_move)(self.mnd_root, self.offset.x, self.offset.y, self.offset.z); + pub fn fix_floor(&mut self, input: &InputState) { + 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 => {} + } } }