From 23b1417275f293a1def979b647eef7fe86e8bfe2 Mon Sep 17 00:00:00 2001
From: Avi Weinstock <aweinstock314@gmail.com>
Date: Sat, 13 Mar 2021 23:18:27 -0500
Subject: [PATCH] Add hermite interpolation for position (disabled for now with
 a flag, since it breaks possession).

---
 common/net/src/sync/interpolation.rs | 45 +++++++++++++++++++++++-----
 common/sys/src/interpolation.rs      |  2 +-
 2 files changed, 38 insertions(+), 9 deletions(-)

diff --git a/common/net/src/sync/interpolation.rs b/common/net/src/sync/interpolation.rs
index b5ffec772a..143c516b9e 100644
--- a/common/net/src/sync/interpolation.rs
+++ b/common/net/src/sync/interpolation.rs
@@ -32,16 +32,19 @@ impl<T: 'static + Send + Sync> Component for InterpBuffer<T> {
 
 // 0 is pure physics, 1 is pure extrapolation
 const PHYSICS_VS_EXTRAPOLATION_FACTOR: f32 = 0.2;
+const POSITION_INTERP_SANITY: f32 = 1000.0;
+const VELOCITY_INTERP_SANITY: f32 = 1000.0;
+const ENABLE_POSITION_HERMITE: bool = false;
 
 impl InterpolatableComponent for Pos {
     type InterpData = InterpBuffer<Pos>;
-    type ReadData = Vel;
+    type ReadData = InterpBuffer<Vel>;
 
     fn update_component(&self, interp_data: &mut Self::InterpData, time: f64) {
         interp_data.push(time, *self);
     }
 
-    fn interpolate(self, interp_data: &Self::InterpData, t2: f64, _vel: &Vel) -> Self {
+    fn interpolate(self, interp_data: &Self::InterpData, t2: f64, vel: &InterpBuffer<Vel>) -> Self {
         // lerp to test interface, do hermite spline later
         let InterpBuffer { ref buf, ref i } = interp_data;
         let (t0, p0) = buf[(i + buf.len() - 1) % buf.len()];
@@ -49,13 +52,35 @@ impl InterpolatableComponent for Pos {
         if (t1 - t0).abs() < f64::EPSILON {
             return self;
         }
-        let lerp_factor = 1.0 + ((t2 - t1) / (t1 - t0)) as f32;
-        let mut out = Lerp::lerp_unclamped(p0.0, p1.0, lerp_factor);
+        if p0.0.distance_squared(p1.0) > POSITION_INTERP_SANITY.powf(2.0) {
+            warn!("position delta exceeded sanity check, clamping");
+            return p1;
+        }
+        let (t0prime, m0) = vel.buf[(i + vel.buf.len() - 1) % vel.buf.len()];
+        let (t1prime, m1) = vel.buf[i % vel.buf.len()];
+        let mut out;
+        let t = (t2 - t0) / (t1 - t0);
+        if ENABLE_POSITION_HERMITE
+            && ((t0 - t0prime).abs() < f64::EPSILON && (t1 - t1prime).abs() < f64::EPSILON)
+        {
+            let h00 = |t: f64| (2.0 * t.powf(3.0) - 3.0 * t.powf(2.0) + 1.0) as f32;
+            let h10 = |t: f64| (t.powf(3.0) - 2.0 * t.powf(2.0) + t) as f32;
+            let h01 = |t: f64| (-2.0 * t.powf(3.0) + 3.0 * t.powf(2.0)) as f32;
+            let h11 = |t: f64| (t.powf(3.0) - t.powf(2.0)) as f32;
+            let dt = (t1 - t0) as f32;
+            out = h00(t) * p0.0 + h10(t) * dt * m0.0 + h01(t) * p1.0 + h11(t) * dt * m1.0;
+        } else {
+            if ENABLE_POSITION_HERMITE {
+                warn!(
+                    "timestamps for pos and vel don't match ({:?}, {:?}), falling back to lerp",
+                    interp_data, vel
+                );
+            }
+            out = Lerp::lerp_unclamped(p0.0, p1.0, t as f32);
+        }
+
         if out.map(|x| x.is_nan()).reduce_or() {
-            warn!(
-                "interpolation output is nan: {}, {}, {:?}",
-                t2, lerp_factor, buf
-            );
+            warn!("interpolation output is nan: {}, {}, {:?}", t2, t, buf);
             out = p1.0;
         }
 
@@ -78,6 +103,10 @@ impl InterpolatableComponent for Vel {
         if (t1 - t0).abs() < f64::EPSILON {
             return self;
         }
+        if p0.0.distance_squared(p1.0) > VELOCITY_INTERP_SANITY.powf(2.0) {
+            warn!("velocity delta exceeded sanity check, clamping");
+            return p1;
+        }
         let lerp_factor = 1.0 + ((t2 - t1) / (t1 - t0)) as f32;
         let mut out = Lerp::lerp_unclamped(p0.0, p1.0, lerp_factor);
         if out.map(|x| x.is_nan()).reduce_or() {
diff --git a/common/sys/src/interpolation.rs b/common/sys/src/interpolation.rs
index 506b949056..3393b80944 100644
--- a/common/sys/src/interpolation.rs
+++ b/common/sys/src/interpolation.rs
@@ -40,7 +40,7 @@ impl<'a> System<'a> for InterpolationSystem {
             &data.entities,
             &mut data.pos,
             &data.pos_interpdata,
-            &data.vel,
+            &data.vel_interpdata,
         )
             .par_join()
             .filter(|(e, _, _, _)| Some(e) != player.as_ref())