From a08c260898f0a4386d6e225e73465a849c19f8ea Mon Sep 17 00:00:00 2001
From: galister <22305755+galister@users.noreply.github.com>
Date: Sun, 10 Mar 2024 12:22:20 +0100
Subject: [PATCH] openvr: chaperone movement fix

---
 Cargo.lock                      |  4 ++--
 src/backend/openvr/playspace.rs | 39 ++++++++++++++++++++++++---------
 2 files changed, 31 insertions(+), 12 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index db20798..729fd97 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -2198,7 +2198,7 @@ dependencies = [
 [[package]]
 name = "ovr_overlay"
 version = "0.0.0"
-source = "git+https://github.com/galister/ovr_overlay_oyasumi#9d8a3f4017024e79b6ec7fa518b97df401bf259c"
+source = "git+https://github.com/galister/ovr_overlay_oyasumi#61e0e77770212d64a76775a8c76637d1ca935942"
 dependencies = [
  "byteorder",
  "derive_more",
@@ -2213,7 +2213,7 @@ dependencies = [
 [[package]]
 name = "ovr_overlay_sys"
 version = "0.0.0"
-source = "git+https://github.com/galister/ovr_overlay_oyasumi#9d8a3f4017024e79b6ec7fa518b97df401bf259c"
+source = "git+https://github.com/galister/ovr_overlay_oyasumi#61e0e77770212d64a76775a8c76637d1ca935942"
 dependencies = [
  "autocxx",
  "autocxx-build",
diff --git a/src/backend/openvr/playspace.rs b/src/backend/openvr/playspace.rs
index cf23ee9..ed60e79 100644
--- a/src/backend/openvr/playspace.rs
+++ b/src/backend/openvr/playspace.rs
@@ -56,20 +56,21 @@ impl PlayspaceMover {
                 return;
             }
 
-            let overlay_offset = data.pose.inverse().transform_vector3a(relative_pos);
+            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 -= overlay_offset;
+                    overlay.state.transform.translation += overlay_offset;
                 }
             });
-            chaperone_mgr.revert_working_copy();
-            apply_chaperone_offset(overlay_offset, chaperone_mgr);
 
             data.pose.translation += relative_pos;
             data.hand_pos = new_hand;
 
+            if self.universe == ETrackingUniverseOrigin::TrackingUniverseStanding {
+                apply_chaperone_offset(overlay_offset, chaperone_mgr);
+            }
             set_working_copy(&universe, chaperone_mgr, &data.pose);
             chaperone_mgr.commit_working_copy(EChaperoneConfigFile::EChaperoneConfigFile_Live);
         } else {
@@ -93,12 +94,19 @@ impl PlayspaceMover {
     }
 
     pub fn reset_offset(&mut self, chaperone_mgr: &mut ChaperoneSetupManager) {
+        if self.universe == ETrackingUniverseOrigin::TrackingUniverseStanding {
+            if let Some(cur) = get_working_copy(&self.universe, chaperone_mgr) {
+                apply_chaperone_transform(cur, chaperone_mgr);
+            }
+        }
         set_working_copy(&self.universe, chaperone_mgr, &Affine3A::IDENTITY);
         chaperone_mgr.commit_working_copy(EChaperoneConfigFile::EChaperoneConfigFile_Live);
 
         if self.last.is_some() {
             log::info!("Space drag interrupted by manual reset");
             self.last = None;
+        } else {
+            log::info!("Playspace reset");
         }
     }
 
@@ -109,7 +117,9 @@ impl PlayspaceMover {
             log::warn!("Can't fix floor - failed to get zero pose");
             return;
         };
-        mat.translation.y += y1.min(y2) - 0.03;
+        let offset = y1.min(y2) - 0.03;
+        mat.translation.y += offset;
+
         set_working_copy(&self.universe, chaperone_mgr, &mat);
         chaperone_mgr.commit_working_copy(EChaperoneConfigFile::EChaperoneConfigFile_Live);
     }
@@ -177,15 +187,24 @@ fn set_working_copy(
 }
 
 fn apply_chaperone_offset(offset: Vec3A, chaperone_mgr: &mut ChaperoneSetupManager) {
-    let mut quads = chaperone_mgr.get_working_collision_bounds_info();
+    let mut quads = chaperone_mgr.get_live_collision_bounds_info();
     quads.iter_mut().for_each(|quad| {
         quad.vCorners.iter_mut().for_each(|corner| {
             corner.v[0] += offset.x;
-            if corner.v[1] != 0.0 {
-                corner.v[1] += offset.y;
-            }
             corner.v[2] += offset.z;
         });
     });
-    chaperone_mgr.set_working_collision_bounds_info(&mut quads);
+    chaperone_mgr.set_working_collision_bounds_info(quads.as_mut_slice());
+}
+
+fn apply_chaperone_transform(transform: Affine3A, chaperone_mgr: &mut ChaperoneSetupManager) {
+    let mut quads = chaperone_mgr.get_live_collision_bounds_info();
+    quads.iter_mut().for_each(|quad| {
+        quad.vCorners.iter_mut().for_each(|corner| {
+            let coord = transform.transform_point3a(Vec3A::from_slice(&corner.v));
+            corner.v[0] = coord.x;
+            corner.v[2] = coord.z;
+        });
+    });
+    chaperone_mgr.set_working_collision_bounds_info(quads.as_mut_slice());
 }