separate floor offset from space drag transform (#218)

Closes #163

src/backend/openvr/playspace.rs

@@ -22,6 +22,7 @@ pub(super) struct PlayspaceMover {
     universe: ETrackingUniverseOrigin,
     drag: Option<MoverData<Vec3A>>,
     rotate: Option<MoverData<Quat>>,
+    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),
     }
 }
 

src/backend/openxr/playspace.rs

@@ -16,6 +16,7 @@ struct MoverData<T> {
 
 pub(super) struct PlayspaceMover {
     last_transform: Affine3A,
+    floor_offset: f32,
     drag: Option<MoverData<Vec3A>>,
     rotate: Option<MoverData<Quat>>,
 }
@@ -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;
+        self.floor_offset = delta;
-        apply_offset(self.last_transform, monado);
+        self.apply_offset(self.last_transform, monado);
     }
-}
 
-fn apply_offset(transform: Affine3A, monado: &mut Monado) {
+    fn apply_offset(&self, transform: Affine3A, monado: &mut Monado) {
         let pose = Pose {
-        position: transform.translation.into(),
+            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);
+    }
 }