diff --git a/src/backend/openvr/playspace.rs b/src/backend/openvr/playspace.rs index 50e16b2..36efcc2 100644 --- a/src/backend/openvr/playspace.rs +++ b/src/backend/openvr/playspace.rs @@ -22,6 +22,7 @@ pub(super) struct PlayspaceMover { universe: ETrackingUniverseOrigin, drag: Option>, rotate: Option>, + floor_offset: f32, } impl PlayspaceMover { @@ -30,6 +31,7 @@ impl PlayspaceMover { universe: ETrackingUniverseOrigin::TrackingUniverseRawAndUncalibrated, drag: None, rotate: None, + floor_offset: 0.0, } } @@ -82,7 +84,7 @@ impl PlayspaceMover { if self.universe == ETrackingUniverseOrigin::TrackingUniverseStanding { apply_chaperone_transform(space_transform.inverse(), chaperone_mgr); } - set_working_copy(&universe, chaperone_mgr, &data.pose); + set_working_copy(&universe, chaperone_mgr, &data.pose, self.floor_offset); chaperone_mgr.commit_working_copy(EChaperoneConfigFile::EChaperoneConfigFile_Live); } else { for (i, pointer) in state.input_state.pointers.iter().enumerate() { @@ -138,7 +140,7 @@ impl PlayspaceMover { if self.universe == ETrackingUniverseOrigin::TrackingUniverseStanding { apply_chaperone_offset(overlay_offset, chaperone_mgr); } - set_working_copy(&universe, chaperone_mgr, &data.pose); + set_working_copy(&universe, chaperone_mgr, &data.pose, self.floor_offset); chaperone_mgr.commit_working_copy(EChaperoneConfigFile::EChaperoneConfigFile_Live); } else { for (i, pointer) in state.input_state.pointers.iter().enumerate() { @@ -176,7 +178,7 @@ impl PlayspaceMover { Affine3A::IDENTITY }; - set_working_copy(&self.universe, chaperone_mgr, &xform); + set_working_copy(&self.universe, chaperone_mgr, &xform, self.floor_offset); chaperone_mgr.commit_working_copy(EChaperoneConfigFile::EChaperoneConfigFile_Live); if self.drag.is_some() { @@ -192,14 +194,14 @@ impl PlayspaceMover { pub fn fix_floor(&mut self, chaperone_mgr: &mut ChaperoneSetupManager, input: &InputState) { let y1 = input.pointers[0].pose.translation.y; let y2 = input.pointers[1].pose.translation.y; - let Some(mut mat) = get_working_copy(&self.universe, chaperone_mgr) else { + let Some(mat) = get_working_copy(&self.universe, chaperone_mgr) else { log::warn!("Can't fix floor - failed to get zero pose"); return; }; let offset = y1.min(y2) - 0.03; - mat.translation.y += offset; + self.floor_offset = offset; - set_working_copy(&self.universe, chaperone_mgr, &mat); + set_working_copy(&self.universe, chaperone_mgr, &mat, self.floor_offset); chaperone_mgr.commit_working_copy(EChaperoneConfigFile::EChaperoneConfigFile_Live); if self.drag.is_some() { @@ -268,13 +270,16 @@ fn set_working_copy( universe: &ETrackingUniverseOrigin, chaperone_mgr: &mut ChaperoneSetupManager, mat: &Affine3A, + floor_offset: f32, ) { - let mat = HmdMatrix34_t::from_affine(mat); + let mut mat = *mat; + mat.translation.y += floor_offset; + let ovr_mat = HmdMatrix34_t::from_affine(&mat); match universe { ETrackingUniverseOrigin::TrackingUniverseStanding => { - chaperone_mgr.set_working_standing_zero_pose_to_raw_tracking_pose(&mat); + chaperone_mgr.set_working_standing_zero_pose_to_raw_tracking_pose(&ovr_mat); } - _ => chaperone_mgr.set_working_seated_zero_pose_to_raw_tracking_pose(&mat), + _ => chaperone_mgr.set_working_seated_zero_pose_to_raw_tracking_pose(&ovr_mat), } } diff --git a/src/backend/openxr/playspace.rs b/src/backend/openxr/playspace.rs index 487c929..e233083 100644 --- a/src/backend/openxr/playspace.rs +++ b/src/backend/openxr/playspace.rs @@ -16,6 +16,7 @@ struct MoverData { pub(super) struct PlayspaceMover { last_transform: Affine3A, + floor_offset: f32, drag: Option>, rotate: Option>, } @@ -36,6 +37,7 @@ impl PlayspaceMover { Ok(Self { last_transform, + floor_offset: 0.0, drag: None, rotate: None, @@ -89,7 +91,7 @@ impl PlayspaceMover { data.pose *= space_transform; data.hand_pose = new_hand; - apply_offset(data.pose, monado); + self.apply_offset(data.pose, monado); self.rotate = Some(data); } else { for (i, pointer) in state.input_state.pointers.iter().enumerate() { @@ -138,7 +140,7 @@ impl PlayspaceMover { data.pose.translation += relative_pos; data.hand_pose = new_hand; - apply_offset(data.pose, monado); + self.apply_offset(data.pose, monado); self.drag = Some(data); } else { for (i, pointer) in state.input_state.pointers.iter().enumerate() { @@ -169,7 +171,7 @@ impl PlayspaceMover { } self.last_transform = Affine3A::IDENTITY; - apply_offset(self.last_transform, monado); + self.apply_offset(self.last_transform, monado); } pub fn fix_floor(&mut self, input: &InputState, monado: &mut Monado) { @@ -185,15 +187,15 @@ impl PlayspaceMover { let y1 = input.pointers[0].raw_pose.translation.y; let y2 = input.pointers[1].raw_pose.translation.y; let delta = y1.min(y2) - 0.03; - self.last_transform.translation.y += delta; - apply_offset(self.last_transform, monado); + self.floor_offset = delta; + self.apply_offset(self.last_transform, monado); + } + + fn apply_offset(&self, transform: Affine3A, monado: &mut Monado) { + let pose = Pose { + position: (transform.translation + Vec3A::new(0.0, self.floor_offset, 0.0)).into(), + orientation: Quat::from_affine3(&transform).into(), + }; + let _ = monado.set_reference_space_offset(ReferenceSpaceType::Stage, pose); } } - -fn apply_offset(transform: Affine3A, monado: &mut Monado) { - let pose = Pose { - position: transform.translation.into(), - orientation: Quat::from_affine3(&transform).into(), - }; - let _ = monado.set_reference_space_offset(ReferenceSpaceType::Stage, pose); -}