use crate::geometry::Point;
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct Velocity {
pub x: f32,
pub y: f32,
}
impl Velocity {
pub const ZERO: Velocity = Velocity { x: 0.0, y: 0.0 };
pub fn new(x: f32, y: f32) -> Self {
Self { x, y }
}
pub fn magnitude(self) -> f32 {
self.x.hypot(self.y)
}
pub fn clamp_magnitude(self, min: f32, max: f32) -> Velocity {
debug_assert!(min >= 0.0 && max >= min);
let magnitude = self.magnitude();
if magnitude == 0.0 {
return self;
}
let target = magnitude.clamp(min, max);
if target == magnitude {
return self;
}
let scale = target / magnitude;
Velocity::new(self.x * scale, self.y * scale)
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct VelocityEstimate {
pub velocity: Velocity,
pub confidence: f32,
pub duration: f32,
pub offset: (f32, f32),
}
impl VelocityEstimate {
pub const STILL: VelocityEstimate = VelocityEstimate {
velocity: Velocity::ZERO,
confidence: 1.0,
duration: 0.0,
offset: (0.0, 0.0),
};
}
const HISTORY: usize = 20;
const HORIZON_MS: f32 = 100.0;
const STOPPED_MS: f32 = 40.0;
const DELIVERY_GAPS: usize = 3;
const MAX_DELIVERY_MS: f32 = 50.0;
const MIN_SAMPLES: usize = 3;
pub const BOUNCING_FLING_WEIGHTS: [f32; 3] = [0.6, 0.35, 0.05];
pub const DESKTOP_FLING_WEIGHTS: [f32; 3] = [0.15, 0.65, 0.2];
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum VelocityStrategy {
Regression,
RecentAverage([f32; 3]),
}
#[derive(Clone, Copy, Debug)]
struct Sample {
point: Point,
time: f32,
}
#[derive(Clone, Debug)]
pub struct VelocityTracker {
strategy: VelocityStrategy,
samples: [Option<Sample>; HISTORY],
index: usize,
}
impl Default for VelocityTracker {
fn default() -> Self {
Self::new(VelocityStrategy::Regression)
}
}
impl VelocityTracker {
pub fn new(strategy: VelocityStrategy) -> Self {
Self {
strategy,
samples: [None; HISTORY],
index: 0,
}
}
pub fn strategy(&self) -> VelocityStrategy {
self.strategy
}
pub fn add_position(&mut self, time: f32, point: Point) {
self.index = (self.index + 1) % HISTORY;
self.samples[self.index] = Some(Sample { point, time });
}
pub fn clear(&mut self) {
self.samples = [None; HISTORY];
self.index = 0;
}
pub fn estimate(&self, now: f32) -> Option<VelocityEstimate> {
let newest = self.samples[self.index]?;
if (now - newest.time) * 1000.0 - self.delivery_ms() > STOPPED_MS {
return Some(VelocityEstimate::STILL);
}
match self.strategy {
VelocityStrategy::Regression => Some(self.regress(newest)),
VelocityStrategy::RecentAverage(weights) => Some(self.recent_average(newest, weights)),
}
}
pub fn velocity(&self, now: f32) -> Velocity {
self.estimate(now)
.map(|e| e.velocity)
.unwrap_or(Velocity::ZERO)
}
fn regress(&self, newest: Sample) -> VelocityEstimate {
let mut ages = [0.0f64; HISTORY];
let mut xs = [0.0f64; HISTORY];
let mut ys = [0.0f64; HISTORY];
let mut count = 0;
let mut oldest = newest;
let mut previous = newest;
let mut index = self.index;
let delivery = self.delivery_ms();
while count < HISTORY {
let Some(sample) = self.samples[index] else {
break;
};
let age = (newest.time - sample.time) * 1000.0;
let gap = ((sample.time - previous.time) * 1000.0).abs();
previous = sample;
if age > HORIZON_MS + delivery || gap - delivery > STOPPED_MS {
break;
}
oldest = sample;
ages[count] = -age as f64;
xs[count] = sample.point.x as f64;
ys[count] = sample.point.y as f64;
count += 1;
index = if index == 0 { HISTORY } else { index } - 1;
}
let duration = newest.time - oldest.time;
let offset = (
newest.point.x - oldest.point.x,
newest.point.y - oldest.point.y,
);
if count >= MIN_SAMPLES {
let x_fit = PolynomialFit::solve(&ages[..count], &xs[..count], 2);
let y_fit = PolynomialFit::solve(&ages[..count], &ys[..count], 2);
if let (Some(x_fit), Some(y_fit)) = (x_fit, y_fit) {
return VelocityEstimate {
velocity: Velocity::new(
(x_fit.coefficients[1] * 1000.0) as f32,
(y_fit.coefficients[1] * 1000.0) as f32,
),
confidence: (x_fit.confidence * y_fit.confidence) as f32,
duration,
offset,
};
}
}
VelocityEstimate {
velocity: Velocity::ZERO,
confidence: 1.0,
duration,
offset,
}
}
fn delivery_ms(&self) -> f32 {
let mut gaps = [0.0f32; DELIVERY_GAPS];
let mut count = 0;
let mut index = self.index;
while count < DELIVERY_GAPS {
let before = if index == 0 { HISTORY } else { index } - 1;
let (Some(newer), Some(older)) = (self.samples[index], self.samples[before]) else {
break;
};
gaps[count] = ((newer.time - older.time) * 1000.0).max(0.0);
count += 1;
index = before;
}
let gaps = &mut gaps[..count];
gaps.sort_by(f32::total_cmp);
let pace = match gaps.len() {
0 => 0.0,
DELIVERY_GAPS => gaps[DELIVERY_GAPS / 2],
_ => gaps[0],
};
pace.min(MAX_DELIVERY_MS)
}
fn pair_velocity(&self, offset: isize) -> Velocity {
let size = HISTORY as isize;
let end = self.samples[((self.index as isize + offset).rem_euclid(size)) as usize];
let start = self.samples[((self.index as isize + offset - 1).rem_euclid(size)) as usize];
let (Some(end), Some(start)) = (end, start) else {
return Velocity::ZERO;
};
let dt = end.time - start.time;
if dt <= 0.0 {
return Velocity::ZERO;
}
Velocity::new(
(end.point.x - start.point.x) / dt,
(end.point.y - start.point.y) / dt,
)
}
fn recent_average(&self, newest: Sample, weights: [f32; 3]) -> VelocityEstimate {
let mut velocity = Velocity::ZERO;
for (i, weight) in weights.iter().enumerate() {
let pair = self.pair_velocity(i as isize - (weights.len() as isize - 1));
velocity.x += pair.x * weight;
velocity.y += pair.y * weight;
}
let mut oldest = newest;
for i in 1..=HISTORY {
if let Some(sample) = self.samples[(self.index + i) % HISTORY] {
oldest = sample;
break;
}
}
VelocityEstimate {
velocity,
confidence: 1.0,
duration: newest.time - oldest.time,
offset: (
newest.point.x - oldest.point.x,
newest.point.y - oldest.point.y,
),
}
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct PolynomialFit {
pub coefficients: [f64; 3],
pub confidence: f64,
}
const PRECISION_TOLERANCE: f64 = 1e-10;
impl PolynomialFit {
pub fn solve(x: &[f64], y: &[f64], degree: usize) -> Option<PolynomialFit> {
debug_assert_eq!(x.len(), y.len());
debug_assert!(degree <= 2);
let m = x.len();
let n = degree + 1;
if degree > m {
return None; }
let mut a = vec![0.0f64; n * m];
for h in 0..m {
a[h] = 1.0;
for i in 1..n {
a[i * m + h] = a[(i - 1) * m + h] * x[h];
}
}
let mut q = vec![0.0f64; n * m];
let mut r = vec![0.0f64; n * n];
for j in 0..n {
for h in 0..m {
q[j * m + h] = a[j * m + h];
}
for i in 0..j {
let dot: f64 = (0..m).map(|h| q[j * m + h] * q[i * m + h]).sum();
for h in 0..m {
q[j * m + h] -= dot * q[i * m + h];
}
}
let norm = (0..m)
.map(|h| q[j * m + h] * q[j * m + h])
.sum::<f64>()
.sqrt();
if norm < PRECISION_TOLERANCE {
return None; }
let inverse = 1.0 / norm;
for h in 0..m {
q[j * m + h] *= inverse;
}
for i in 0..n {
r[j * n + i] = if i < j {
0.0
} else {
(0..m).map(|h| q[j * m + h] * a[i * m + h]).sum()
};
}
}
let mut coefficients = [0.0f64; 3];
for i in (0..n).rev() {
let mut c: f64 = (0..m).map(|h| q[i * m + h] * y[h]).sum();
for j in (i + 1..n).rev() {
c -= r[i * n + j] * coefficients[j];
}
coefficients[i] = c / r[i * n + i];
}
let mean = y.iter().sum::<f64>() / m as f64;
let mut squared_error = 0.0;
let mut squared_total = 0.0;
for h in 0..m {
let mut term = 1.0;
let mut error = y[h] - coefficients[0];
for c in coefficients.iter().take(n).skip(1) {
term *= x[h];
error -= term * c;
}
squared_error += error * error;
let deviation = y[h] - mean;
squared_total += deviation * deviation;
}
let confidence = if squared_total <= PRECISION_TOLERANCE {
1.0
} else {
1.0 - (squared_error / squared_total)
};
Some(PolynomialFit {
coefficients,
confidence,
})
}
}
#[cfg(test)]
mod tests {
use super::*;
fn steady(tracker: &mut VelocityTracker, speed: f32, samples: usize) -> f32 {
let step = 0.008;
let mut t = 0.0;
for i in 0..samples {
t = i as f32 * step;
tracker.add_position(t, Point::new(speed * t, 2.0 * speed * t));
}
t
}
fn feed(tracker: &mut VelocityTracker, deltas: &[f32]) -> f32 {
let step = 0.008;
let mut t = 0.0;
let mut x = 0.0;
tracker.add_position(t, Point::new(x, 0.0));
for delta in deltas {
t += step;
x += delta;
tracker.add_position(t, Point::new(x, 0.0));
}
t
}
fn throw_then_lift(tracker: &mut VelocityTracker, slow: usize) -> f32 {
let mut deltas = vec![8.0f32; 7];
deltas.extend(std::iter::repeat_n(1.0f32, slow));
feed(tracker, &deltas)
}
#[test]
fn a_steady_drag_is_read_exactly() {
let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
let now = steady(&mut tracker, 600.0, 10);
let estimate = tracker.estimate(now).expect("samples were added");
assert!(
(estimate.velocity.x - 600.0).abs() < 1.0,
"x = {}",
estimate.velocity.x
);
assert!(
(estimate.velocity.y - 1200.0).abs() < 2.0,
"y = {}",
estimate.velocity.y
);
assert!(estimate.confidence > 0.99, "a line fits a line");
assert!(estimate.offset.0 > 0.0 && estimate.duration > 0.0);
}
#[test]
fn the_regression_survives_a_last_instant_slowdown() {
let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
let now = throw_then_lift(&mut tracker, 2);
let estimate = tracker.estimate(now).expect("samples");
assert!(
estimate.velocity.x > 400.0,
"the throw should survive the lift-off, got {}",
estimate.velocity.x
);
}
#[test]
fn a_finger_that_stopped_before_lifting_flings_nothing() {
let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
let last = steady(&mut tracker, 900.0, 8);
let estimate = tracker.estimate(last + 0.1).expect("samples");
assert_eq!(estimate.velocity, Velocity::ZERO);
assert_eq!(estimate, VelocityEstimate::STILL);
}
fn paced(tracker: &mut VelocityTracker, speed: f32, step: f32, samples: usize) -> f32 {
let mut t = 0.0;
for i in 0..samples {
t = i as f32 * step;
tracker.add_position(t, Point::new(speed * t, 0.0));
}
t
}
#[test]
fn a_throw_delivered_a_frame_at_a_time_is_still_a_throw() {
for strategy in [
VelocityStrategy::Regression,
VelocityStrategy::RecentAverage(BOUNCING_FLING_WEIGHTS),
] {
for step in [0.037, 0.045, 0.051] {
let mut tracker = VelocityTracker::new(strategy);
let last = paced(&mut tracker, 2000.0, step, 6);
let estimate = tracker.estimate(last + 0.041).expect("samples");
assert!(
(estimate.velocity.x - 2000.0).abs() < 40.0,
"{strategy:?} every {step} s: got {}",
estimate.velocity.x
);
}
}
}
#[test]
fn a_short_throw_at_slow_frames_is_still_a_throw() {
let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
let last = paced(&mut tracker, 2000.0, 0.037, 3);
let estimate = tracker.estimate(last + 0.041).expect("samples");
assert!(
(estimate.velocity.x - 2000.0).abs() < 40.0,
"got {}",
estimate.velocity.x
);
}
#[test]
fn slow_frames_do_not_hide_a_finger_that_stopped() {
let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
let last = paced(&mut tracker, 2000.0, 0.037, 6);
let estimate = tracker.estimate(last + 0.12).expect("samples");
assert_eq!(estimate, VelocityEstimate::STILL);
let mut crawling = VelocityTracker::new(VelocityStrategy::Regression);
let last = paced(&mut crawling, 2000.0, 0.2, 6);
assert_eq!(
crawling.estimate(last + 0.1).expect("samples"),
VelocityEstimate::STILL
);
}
#[test]
fn one_short_gap_does_not_change_the_pace() {
let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
for (t, x) in [
(0.0, 0.0),
(0.037, 74.0),
(0.074, 148.0),
(0.111, 222.0),
(0.116, 232.0),
(0.153, 306.0),
] {
tracker.add_position(t, Point::new(x, 0.0));
}
let estimate = tracker.estimate(0.153 + 0.05).expect("samples");
assert!(estimate.velocity.x > 1500.0, "got {}", estimate.velocity.x);
}
#[test]
fn a_pause_just_before_the_release_excuses_nothing() {
let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
let mut t = 0.0;
for i in 0..6 {
t = i as f32 * 0.008;
tracker.add_position(t, Point::new(i as f32 * 16.0, 0.0));
}
let stopped = t + 0.06;
tracker.add_position(stopped, Point::new(80.0, 0.0));
let estimate = tracker.estimate(stopped + 0.005).expect("samples");
assert!(
estimate.velocity.x < 400.0,
"the throw before the pause leaked across it: {}",
estimate.velocity.x
);
assert!(
estimate.offset.0.abs() < 1.0 && estimate.duration < 0.01,
"the history ran back across the pause: {estimate:?}"
);
}
#[test]
fn samples_older_than_the_horizon_are_not_part_of_the_motion() {
let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
for i in 0..6 {
tracker.add_position(i as f32 * 0.008, Point::new(i as f32 * 16.0, 0.0));
}
let base = 1.0;
for i in 0..6 {
tracker.add_position(
base + i as f32 * 0.008,
Point::new(1000.0 + i as f32 * 1.6, 0.0),
);
}
let now = base + 5.0 * 0.008;
let estimate = tracker.estimate(now).expect("samples");
assert!(
estimate.velocity.x < 400.0,
"the old fast gesture leaked in: {}",
estimate.velocity.x
);
assert!(estimate.duration < 0.1, "span = {}", estimate.duration);
}
#[test]
fn too_few_samples_give_travel_but_no_speed() {
let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
tracker.add_position(0.0, Point::new(0.0, 0.0));
tracker.add_position(0.008, Point::new(10.0, 0.0));
let estimate = tracker.estimate(0.008).expect("samples");
assert_eq!(
estimate.velocity,
Velocity::ZERO,
"2 samples cannot regress"
);
assert_eq!(estimate.offset.0, 10.0, "but the travel is still known");
}
#[test]
fn an_empty_tracker_has_nothing_to_say() {
let tracker = VelocityTracker::new(VelocityStrategy::Regression);
assert!(tracker.estimate(0.0).is_none());
assert_eq!(tracker.velocity(0.0), Velocity::ZERO);
}
#[test]
fn clearing_starts_the_gesture_over() {
let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
let now = steady(&mut tracker, 600.0, 10);
tracker.clear();
assert!(tracker.estimate(now).is_none());
}
#[test]
fn the_recent_average_leans_on_the_older_samples() {
let mut tracker =
VelocityTracker::new(VelocityStrategy::RecentAverage(BOUNCING_FLING_WEIGHTS));
let now = throw_then_lift(&mut tracker, 1);
let estimate = tracker.estimate(now).expect("samples");
assert!(
(estimate.velocity.x - 956.0).abs() < 20.0,
"got {}",
estimate.velocity.x
);
}
#[test]
fn the_recent_average_needs_no_history_to_answer() {
let mut tracker =
VelocityTracker::new(VelocityStrategy::RecentAverage(DESKTOP_FLING_WEIGHTS));
tracker.add_position(0.0, Point::new(0.0, 0.0));
tracker.add_position(0.008, Point::new(8.0, 0.0));
let estimate = tracker.estimate(0.008).expect("samples");
assert!(
(estimate.velocity.x - 200.0).abs() < 1.0,
"got {}",
estimate.velocity.x
);
}
#[test]
fn the_ring_buffer_wraps_without_losing_the_motion() {
let mut tracker = VelocityTracker::new(VelocityStrategy::Regression);
let now = steady(&mut tracker, 500.0, HISTORY * 3);
let estimate = tracker.estimate(now).expect("samples");
assert!(
(estimate.velocity.x - 500.0).abs() < 2.0,
"got {}",
estimate.velocity.x
);
}
#[test]
fn a_quadratic_is_recovered_exactly() {
let x: Vec<f64> = (0..8).map(|i| i as f64 * 0.5).collect();
let y: Vec<f64> = x.iter().map(|x| 3.0 + 2.0 * x + 4.0 * x * x).collect();
let fit = PolynomialFit::solve(&x, &y, 2).expect("a well-posed system");
assert!((fit.coefficients[0] - 3.0).abs() < 1e-6, "{:?}", fit);
assert!((fit.coefficients[1] - 2.0).abs() < 1e-6, "{:?}", fit);
assert!((fit.coefficients[2] - 4.0).abs() < 1e-6, "{:?}", fit);
assert!((fit.confidence - 1.0).abs() < 1e-9);
}
#[test]
fn a_degenerate_system_has_no_fit() {
let x = [1.0, 1.0, 1.0, 1.0];
let y = [0.0, 1.0, 2.0, 3.0];
assert!(PolynomialFit::solve(&x, &y, 2).is_none());
}
#[test]
fn confidence_falls_when_the_points_do_not_follow_the_curve() {
let x: Vec<f64> = (0..10).map(|i| i as f64).collect();
let clean: Vec<f64> = x.iter().map(|x| 2.0 * x).collect();
let noisy: Vec<f64> = x
.iter()
.enumerate()
.map(|(i, x)| 2.0 * x + if i % 2 == 0 { 40.0 } else { -40.0 })
.collect();
let a = PolynomialFit::solve(&x, &clean, 2).unwrap();
let b = PolynomialFit::solve(&x, &noisy, 2).unwrap();
assert!(
a.confidence > b.confidence,
"{} vs {}",
a.confidence,
b.confidence
);
assert!(b.confidence < 0.9);
}
#[test]
fn a_velocity_can_be_brought_into_range() {
let v = Velocity::new(3000.0, 4000.0); assert!((v.magnitude() - 5000.0).abs() < 1e-3);
let capped = v.clamp_magnitude(0.0, 1000.0);
assert!((capped.magnitude() - 1000.0).abs() < 1e-2);
assert!((capped.x / capped.y - v.x / v.y).abs() < 1e-4);
assert_eq!(v.clamp_magnitude(0.0, 8000.0), v);
assert_eq!(Velocity::ZERO.clamp_magnitude(50.0, 100.0), Velocity::ZERO);
}
}