From 16a01a0ee3c342dd4a8511fa47c8058bffeef6f8 Mon Sep 17 00:00:00 2001
From: Riccardo Zaglia <riccardo.zaglia5@gmail.com>
Date: Sat, 19 Oct 2024 21:47:01 +0200
Subject: [PATCH] tracking rewrite (3): server-side prediction (disabled)

---
 alvr/server_core/src/lib.rs          | 30 ++++++++++++++++++++++------
 alvr/server_core/src/tracking/mod.rs | 29 +++++++++++++++++++++++++++
 alvr/server_openvr/src/lib.rs        | 12 +++++++----
 3 files changed, 61 insertions(+), 10 deletions(-)

diff --git a/alvr/server_core/src/lib.rs b/alvr/server_core/src/lib.rs
index 398e717887..2b2d90e980 100644
--- a/alvr/server_core/src/lib.rs
+++ b/alvr/server_core/src/lib.rs
@@ -258,6 +258,22 @@ impl ServerCoreContext {
             .get_device_motion(device_id, sample_timestamp)
     }
 
+    pub fn get_predicted_device_motion(
+        &self,
+        device_id: u64,
+        sample_timestamp: Duration,
+        target_timestamp: Duration,
+    ) -> Option<DeviceMotion> {
+        dbg_server_core!(
+            "get_predicted_device_motion: dev={device_id} sample_ts={sample_timestamp:?} target_ts={target_timestamp:?}"
+        );
+
+        self.connection_context
+            .tracking_manager
+            .read()
+            .get_predicted_device_motion(device_id, sample_timestamp, target_timestamp)
+    }
+
     pub fn get_hand_skeleton(
         &self,
         hand_type: HandType,
@@ -288,12 +304,14 @@ impl ServerCoreContext {
     pub fn get_tracker_pose_time_offset(&self) -> Duration {
         dbg_server_core!("get_tracker_pose_time_offset");
 
-        self.connection_context
-            .statistics_manager
-            .read()
-            .as_ref()
-            .map(|stats| stats.tracker_pose_time_offset())
-            .unwrap_or_default()
+        // self.connection_context
+        //     .statistics_manager
+        //     .read()
+        //     .as_ref()
+        //     .map(|stats| stats.tracker_pose_time_offset())
+        //     .unwrap_or_default()
+
+        Duration::from_millis(0)
     }
 
     pub fn send_haptics(&self, haptics: Haptics) {
diff --git a/alvr/server_core/src/tracking/mod.rs b/alvr/server_core/src/tracking/mod.rs
index 3a6ce3b426..d29aeeefb9 100644
--- a/alvr/server_core/src/tracking/mod.rs
+++ b/alvr/server_core/src/tracking/mod.rs
@@ -245,6 +245,35 @@ impl TrackingManager {
             })
     }
 
+    pub fn get_predicted_device_motion(
+        &self,
+        device_id: u64,
+        sample_timestamp: Duration,
+        target_timestamp: Duration,
+    ) -> Option<DeviceMotion> {
+        let motion = self.get_device_motion(device_id, sample_timestamp)?;
+
+        // There is no simple sub for Duration, this is needed to get signed difference
+        let delta_time_s = target_timestamp
+            .saturating_sub(sample_timestamp)
+            .as_secs_f32()
+            - sample_timestamp
+                .saturating_sub(target_timestamp)
+                .as_secs_f32();
+
+        let delta_position = motion.linear_velocity * delta_time_s;
+        let delta_orientation = Quat::from_scaled_axis(motion.angular_velocity * delta_time_s);
+
+        Some(DeviceMotion {
+            pose: Pose {
+                orientation: delta_orientation * motion.pose.orientation,
+                position: motion.pose.position + delta_position,
+            },
+            linear_velocity: motion.linear_velocity,
+            angular_velocity: motion.angular_velocity,
+        })
+    }
+
     pub fn report_hand_skeleton(
         &mut self,
         hand_type: HandType,
diff --git a/alvr/server_openvr/src/lib.rs b/alvr/server_openvr/src/lib.rs
index 6445d588cc..6284f7aa0e 100644
--- a/alvr/server_openvr/src/lib.rs
+++ b/alvr/server_openvr/src/lib.rs
@@ -121,17 +121,21 @@ extern "C" fn driver_ready_idle(set_default_chap: bool) {
                         .unwrap_or(false);
 
                     if let Some(context) = &*SERVER_CORE_CONTEXT.read() {
+                        let target_timestamp =
+                            sample_timestamp + context.get_motion_to_photon_latency();
                         let controllers_pose_time_offset = context.get_tracker_pose_time_offset();
+                        let controllers_timestamp =
+                            target_timestamp.saturating_sub(controllers_pose_time_offset);
 
                         let ffi_head_motion = context
-                            .get_device_motion(*HEAD_ID, sample_timestamp)
+                            .get_device_motion(*HEAD_ID, target_timestamp)
                             .map(|m| tracking::to_ffi_motion(*HEAD_ID, m))
                             .unwrap_or_else(FfiDeviceMotion::default);
                         let ffi_left_controller_motion = context
-                            .get_device_motion(*HAND_LEFT_ID, sample_timestamp)
+                            .get_device_motion(*HAND_LEFT_ID, controllers_timestamp)
                             .map(|m| tracking::to_ffi_motion(*HAND_LEFT_ID, m));
                         let ffi_right_controller_motion = context
-                            .get_device_motion(*HAND_RIGHT_ID, sample_timestamp)
+                            .get_device_motion(*HAND_RIGHT_ID, controllers_timestamp)
                             .map(|m| tracking::to_ffi_motion(*HAND_RIGHT_ID, m));
 
                         let (
@@ -222,7 +226,7 @@ extern "C" fn driver_ready_idle(set_default_chap: bool) {
                         // independently. Selection is done by setting deviceIsConnected.
                         unsafe {
                             SetTracking(
-                                sample_timestamp.as_nanos() as _,
+                                target_timestamp.as_nanos() as _,
                                 controllers_pose_time_offset.as_secs_f32(),
                                 ffi_head_motion,
                                 ffi_left_hand_data,