veloren_common_net/sync/
interpolation.rs

1// impls of `InterpolatableComponent` on things defined in `common`, since
2// `common_net` is downstream of `common`, and an `InterpolationSystem` that
3// applies them
4use super::InterpolatableComponent;
5use common::comp::{Ori, Pos, Vel};
6use specs::Component;
7use tracing::warn;
8use vek::ops::{Lerp, Slerp};
9
10#[derive(Debug)]
11pub struct InterpBuffer<T> {
12    pub buf: [(f64, T); 4],
13    pub i: usize,
14}
15
16impl<T: Clone> InterpBuffer<T> {
17    pub fn new(x: T) -> Self {
18        Self {
19            buf: [
20                (0.0, x.clone()),
21                (0.0, x.clone()),
22                (0.0, x.clone()),
23                (0.0, x),
24            ],
25            i: 0,
26        }
27    }
28
29    fn push(&mut self, time: f64, x: T) {
30        let InterpBuffer { buf, i } = self;
31        *i += 1;
32        *i %= buf.len();
33        buf[*i] = (time, x);
34    }
35
36    fn force_update(&mut self, time: f64, x: T) {
37        for i in 0..self.buf.len() {
38            self.buf[i] = (time, x.clone());
39        }
40    }
41
42    fn update(&mut self, time: f64, x: T, force_update: bool) {
43        if force_update {
44            self.force_update(time, x);
45        } else {
46            self.push(time, x);
47        }
48    }
49}
50
51impl<T: 'static + Send + Sync> Component for InterpBuffer<T> {
52    type Storage = specs::VecStorage<Self>;
53}
54
55// 0 is pure physics, 1 is pure extrapolation
56const PHYSICS_VS_EXTRAPOLATION_FACTOR: f32 = 0.1;
57const POSITION_INTERP_SANITY: Option<f32> = None;
58const VELOCITY_INTERP_SANITY: Option<f32> = None;
59const ENABLE_POSITION_HERMITE: bool = false;
60
61impl InterpolatableComponent for Pos {
62    type InterpData = InterpBuffer<Pos>;
63    type ReadData = InterpBuffer<Vel>;
64
65    fn new_data(x: Self) -> Self::InterpData { InterpBuffer::new(x) }
66
67    fn update_component(&self, interp_data: &mut Self::InterpData, time: f64, force_update: bool) {
68        interp_data.update(time, *self, force_update);
69    }
70
71    fn interpolate(self, interp_data: &Self::InterpData, t2: f64, vel: &InterpBuffer<Vel>) -> Self {
72        // lerp to test interface, do hermite spline later
73        let InterpBuffer { buf, i } = interp_data;
74        let (t0, p0) = buf[(i + buf.len() - 1) % buf.len()];
75        let (t1, p1) = buf[i % buf.len()];
76        if (t1 - t0).abs() < f64::EPSILON {
77            return self;
78        }
79        if POSITION_INTERP_SANITY.is_some_and(|limit| p0.0.distance_squared(p1.0) > limit.powf(2.0))
80        {
81            warn!("position delta exceeded sanity check, clamping");
82            return p1;
83        }
84        let (t0prime, m0) = vel.buf[(i + vel.buf.len() - 1) % vel.buf.len()];
85        let (t1prime, m1) = vel.buf[i % vel.buf.len()];
86        let t = (t2 - t0) / (t1 - t0);
87        let mut out = if ENABLE_POSITION_HERMITE
88            && ((t0 - t0prime).abs() < f64::EPSILON && (t1 - t1prime).abs() < f64::EPSILON)
89        {
90            let h00 = |t: f64| (2.0 * t.powf(3.0) - 3.0 * t.powf(2.0) + 1.0) as f32;
91            let h10 = |t: f64| (t.powf(3.0) - 2.0 * t.powf(2.0) + t) as f32;
92            let h01 = |t: f64| (-2.0 * t.powf(3.0) + 3.0 * t.powf(2.0)) as f32;
93            let h11 = |t: f64| (t.powf(3.0) - t.powf(2.0)) as f32;
94            let dt = (t1 - t0) as f32;
95            h00(t) * p0.0 + h10(t) * dt * m0.0 + h01(t) * p1.0 + h11(t) * dt * m1.0
96        } else {
97            if ENABLE_POSITION_HERMITE {
98                warn!(
99                    "timestamps for pos and vel don't match ({:?}, {:?}), falling back to lerp",
100                    interp_data, vel
101                );
102            }
103            Lerp::lerp_unclamped(p0.0, p1.0, t as f32)
104        };
105
106        if out.map(|x| x.is_nan()).reduce_or() {
107            warn!("interpolation output is nan: {}, {}, {:?}", t2, t, buf);
108            out = p1.0;
109        }
110
111        Pos(Lerp::lerp(self.0, out, PHYSICS_VS_EXTRAPOLATION_FACTOR))
112    }
113}
114
115impl InterpolatableComponent for Vel {
116    type InterpData = InterpBuffer<Vel>;
117    type ReadData = ();
118
119    fn new_data(x: Self) -> Self::InterpData { InterpBuffer::new(x) }
120
121    fn update_component(&self, interp_data: &mut Self::InterpData, time: f64, force_update: bool) {
122        interp_data.update(time, *self, force_update);
123    }
124
125    fn interpolate(self, interp_data: &Self::InterpData, t2: f64, _: &()) -> Self {
126        let InterpBuffer { buf, i } = interp_data;
127        let (t0, p0) = buf[(i + buf.len() - 1) % buf.len()];
128        let (t1, p1) = buf[i % buf.len()];
129        if (t1 - t0).abs() < f64::EPSILON {
130            return self;
131        }
132        if VELOCITY_INTERP_SANITY.is_some_and(|limit| p0.0.distance_squared(p1.0) > limit.powf(2.0))
133        {
134            warn!("velocity delta exceeded sanity check, clamping");
135            return p1;
136        }
137        let lerp_factor = 1.0 + ((t2 - t1) / (t1 - t0)) as f32;
138        let mut out = Lerp::lerp_unclamped(p0.0, p1.0, lerp_factor);
139        if out.map(|x| x.is_nan()).reduce_or() {
140            warn!(
141                "interpolation output is nan: {}, {}, {:?}",
142                t2, lerp_factor, buf
143            );
144            out = p1.0;
145        }
146
147        Vel(Lerp::lerp(self.0, out, PHYSICS_VS_EXTRAPOLATION_FACTOR))
148    }
149}
150
151impl InterpolatableComponent for Ori {
152    type InterpData = InterpBuffer<Ori>;
153    type ReadData = ();
154
155    fn new_data(x: Self) -> Self::InterpData { InterpBuffer::new(x) }
156
157    fn update_component(&self, interp_data: &mut Self::InterpData, time: f64, force_update: bool) {
158        interp_data.update(time, *self, force_update);
159    }
160
161    fn interpolate(self, interp_data: &Self::InterpData, t2: f64, _: &()) -> Self {
162        let InterpBuffer { buf, i } = interp_data;
163        let (t0, p0) = buf[(i + buf.len() - 1) % buf.len()];
164        let (t1, p1) = buf[i % buf.len()];
165        if (t1 - t0).abs() < f64::EPSILON {
166            return self;
167        }
168        let lerp_factor = 1.0 + ((t2 - t1) / (t1 - t0)) as f32;
169        let mut out = Slerp::slerp_unclamped(p0.to_quat(), p1.to_quat(), lerp_factor);
170        if out.into_vec4().map(|x| x.is_nan()).reduce_or() {
171            warn!(
172                "interpolation output is nan: {}, {}, {:?}",
173                t2, lerp_factor, buf
174            );
175            out = p1.to_quat();
176        }
177
178        Ori::new(Slerp::slerp(self.to_quat(), out, PHYSICS_VS_EXTRAPOLATION_FACTOR).normalized())
179    }
180}
veloren_common_net/sync/interpolation.rs

veloren_common_net/sync/
interpolation.rs
