impl Default for InputPredictor {
fn default() -> Self {
Self::new()
}
}
pub struct InputPredictor {
history_x: [f32; 3],
history_y: [f32; 3],
history_time: [f64; 3],
count: usize,
}
impl InputPredictor {
pub fn new() -> Self {
Self {
history_x: [0.0; 3],
history_y: [0.0; 3],
history_time: [0.0; 3],
count: 0,
}
}
pub fn add_sample(&mut self, x: f32, y: f32, time: f64) {
self.history_x[2] = self.history_x[1];
self.history_x[1] = self.history_x[0];
self.history_x[0] = x;
self.history_y[2] = self.history_y[1];
self.history_y[1] = self.history_y[0];
self.history_y[0] = y;
self.history_time[2] = self.history_time[1];
self.history_time[1] = self.history_time[0];
self.history_time[0] = time;
if self.count < 3 {
self.count += 1;
}
}
pub fn predict_at(&self, target_time: f64) -> (f32, f32) {
if self.count < 2 {
return (self.history_x[0], self.history_y[0]);
}
let t0 = self.history_time[0];
let t1 = self.history_time[1];
if (t0 - t1).abs() < 0.0001 {
return (self.history_x[0], self.history_y[0]);
}
let vx = (self.history_x[0] - self.history_x[1]) / (t0 - t1) as f32;
let vy = (self.history_y[0] - self.history_y[1]) / (t0 - t1) as f32;
let dt = (target_time - t0) as f32;
(self.history_x[0] + vx * dt, self.history_y[0] + vy * dt)
}
}